import os
import struct
import rospy
import sys
import time
import threading
import subprocess
import math
import json
import re
import socket
import serial
import can
from datetime import datetime
from PyQt5 import QtCore, QtGui
from PyQt5.QtWidgets import QApplication, QMainWindow, QWidget, QComboBox, QLineEdit, QListView, QCompleter, QFileDialog
from PyQt5.QtCore import QThread, pyqtSignal, Qt, QTime, QTimer, QObject
from PyQt5.QtGui import QStandardItemModel, QStandardItem, QMouseEvent
from widget import Ui_widget_main, Ui_widget_standly
from paho.mqtt import client as mqtt_client
import pyqtgraph as pg
from parameter import AgvParameters, DeviceParameters
import pandas as pd
from flask import Flask, render_template, request
import imu
from std_msgs.msg import String, Float32
# region # 定义全局变量
AgvState = {}
virtual_machine = False
controller = "jihecheng"  # RK3568\RK3588
# endregion
# region # 自定义函数及类


def extract_ip():  # 获取本机IP
    st = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    try:
        st.connect(('10.255.255.255', 1))
        IP = st.getsockname()[0]
    except Exception:
        IP = '127.0.0.1'
    finally:
        st.close()
    return IP


def HexStrAddSpace(HexStr):  # 十六进制字符串标准化打印
    out_data = ''
    for i, s in enumerate(HexStr):
        if ((i % 2 == 0) & (i > 0)):
            out_data += ' '
            out_data += s
        else:
            out_data += s
    return out_data.upper()


def show_text(function):

    def wrapped(self, *args, **kwargs):
        if self.vars["showTextLock"]:
            self.vars["showTextLock"] = False
            result = function(self, *args, **kwargs)
            items = self.get_selected()
            l = len(items)
            l_ = self.vars["listViewModel"].rowCount() - 1
            self.vars["listViewModel"].item(0).setCheckState(
                Qt.Checked if l == l_ else Qt.Unchecked if l == 0 else Qt.PartiallyChecked)
            self.vars["lineEdit"].setText("(全选)" if l == l_ else "(无选择)" if l == 0 else ";".join(
                (item.text() for item in items)))
            self.vars["showTextLock"] = True
        else:
            result = function(self, *args, **kwargs)
        return result

    return wrapped


class QComboCheckBox(QComboBox):

    class MyListView(QListView):

        def __init__(self, parent: QWidget = None, vars=None):
            super().__init__(parent)
            self.vars = vars

        def mousePressEvent(self, event: QMouseEvent):
            self.vars["lock"] = False
            super().mousePressEvent(event)

        def mouseDoubleClickEvent(self, event: QMouseEvent):
            self.vars["lock"] = False
            super().mouseDoubleClickEvent(event)

    def __init__(self, parent: QWidget = None):
        super().__init__(parent)
        self.vars = dict()
        self.vars["lock"] = True
        self.vars["showTextLock"] = True
        # 装饰器锁，避免批量操作时重复改变lineEdit的显示
        self.vars["lineEdit"] = QLineEdit(self)
        self.vars["lineEdit"].setReadOnly(True)
        self.vars["listView"] = self.MyListView(self, self.vars)
        self.vars["listViewModel"] = QStandardItemModel(self)
        self.setModel(self.vars["listViewModel"])
        self.setView(self.vars["listView"])
        self.setLineEdit(self.vars["lineEdit"])

        self.activated.connect(self.__show_selected)

        self.add_item("(全选)")

    def count(self):
        # 返回子项数
        return super().count() - 1

    @show_text
    def add_item(self, text: "str"):
        # 根据文本添加子项
        item = QStandardItem()
        item.setText(text)
        item.setCheckable(True)
        item.setCheckState(Qt.Checked)
        self.vars["listViewModel"].appendRow(item)

    @show_text
    def add_items(self, texts: "tuple or list"):
        # 根据文本列表添加子项
        for text in texts:
            self.add_item(text)

    @show_text
    def clear_items(self):
        # 清空所有子项
        self.vars["listViewModel"].clear()
        self.add_item("(全选)")

    def find_index(self, index: "int"):
        # 根据索引查找子项
        return self.vars["listViewModel"].item(index if index < 0 else index + 1)

    def find_indexs(self, indexs: "tuple or list"):
        # 根据索引列表查找子项
        return [self.find_index(index) for index in indexs]

    def find_text(self, text: "str"):
        # 根据文本查找子项
        tempList = self.vars["listViewModel"].findItems(text)
        tempList.pop(0) if tempList and tempList[0].row() == 0 else tempList
        return tempList

    def find_texts(self, texts: "tuple or list"):
        # 根据文本列表查找子项
        return {text: self.find_text(text) for text in texts}

    def get_text(self, index: "int"):
        # 根据索引返回文本
        return self.vars["listViewModel"].item(index if index < 0 else index + 1).text()

    def get_texts(self, indexs: "tuple or list"):
        # 根据索引列表返回文本
        return [self.get_text(index) for index in indexs]

    def change_text(self, index: "int", text: "str"):
        # 根据索引改变某一子项的文本
        self.vars["listViewModel"].item(
            index if index < 0 else index + 1).setText(text)

    @show_text
    def select_index(self, index: "int", state: "bool" = True):
        # 根据索引选中子项，state=False时为取消选中
        self.vars["listViewModel"].item(
            index if index < 0 else index + 1).setCheckState(Qt.Checked if state else Qt.Unchecked)

    @show_text
    def select_indexs(self, indexs: "tuple or list", state: "bool" = True):
        # 根据索引列表选中子项，state=False时为取消选中
        for index in indexs:
            self.select_index(index, state)

    @show_text
    def select_text(self, text: "str", state: "bool" = True):
        # 根据文本选中子项，state=False时为取消选中
        for item in self.find_text(text):
            item.setCheckState(Qt.Checked if state else Qt.Unchecked)

    @show_text
    def select_texts(self, texts: "tuple or list", state: "bool" = True):
        # 根据文本列表选中子项，state=False时为取消选中
        for text in texts:
            self.select_text(text, state)

    @show_text
    def select_reverse(self):
        # 反转选择
        if self.vars["listViewModel"].item(0).checkState() == Qt.Unchecked:
            self.select_all()
        elif self.vars["listViewModel"].item(0).checkState() == Qt.Checked:
            self.select_clear()
        else:
            for row in range(1, self.vars["listViewModel"].rowCount()):
                self.__select_reverse(row)

    def __select_reverse(self, row: "int"):
        item = self.vars["listViewModel"].item(row)
        item.setCheckState(Qt.Unchecked if item.checkState()
                           == Qt.Checked else Qt.Checked)

    @show_text
    def select_all(self):
        # 全选
        for row in range(0, self.vars["listViewModel"].rowCount()):
            self.vars["listViewModel"].item(row).setCheckState(Qt.Checked)

    @show_text
    def select_clear(self):
        # 全不选
        for row in range(0, self.vars["listViewModel"].rowCount()):
            self.vars["listViewModel"].item(row).setCheckState(Qt.Unchecked)

    @show_text
    def remove_index(self, index: "int"):
        # 根据索引移除子项
        return self.vars["listViewModel"].takeRow(index if index < 0 else index + 1)

    @show_text
    def remove_indexs(self, indexs: "tuple or list"):
        # 根据索引列表移除子项
        return [self.remove_index(index) for index in sorted(indexs, reverse=True)]

    @show_text
    def remove_text(self, text: "str"):
        # 根据文本移除子项
        items = self.find_text(text)
        indexs = [item.row() for item in items]
        return [self.vars["listViewModel"].takeRow(index) for index in sorted(indexs, reverse=True)]

    @show_text
    def remove_texts(self, texts: "tuple or list"):
        # 根据文本列表移除子项
        return {text: self.remove_text(text) for text in texts}

    def get_selected(self):
        # 获取当前选择的子项
        items = list()
        for row in range(1, self.vars["listViewModel"].rowCount()):
            item = self.vars["listViewModel"].item(row)
            if item.checkState() == Qt.Checked:
                items.append(item)
        return items

    def is_all(self):
        # 判断是否是全选
        return True if self.vars["listViewModel"].item(0).checkState() == Qt.Checked else False

    def sort(self, order=Qt.AscendingOrder):
        # 排序，默认正序
        self.vars["listViewModel"].sort(0, order)

    @show_text
    def __show_selected(self, index):
        if not self.vars["lock"]:
            if index == 0:
                if self.vars["listViewModel"].item(0).checkState() == Qt.Checked:
                    self.select_clear()
                else:
                    self.select_all()
            else:
                self.__select_reverse(index)

            self.vars["lock"] = True

    def hidePopup(self):
        if self.vars["lock"]:
            super().hidePopup()


def extract_ip():  # 获取本机IP
    st = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    try:
        st.connect(('10.255.255.255', 1))
        IP = st.getsockname()[0]
    except Exception:
        IP = '127.0.0.1'
    finally:
        st.close()
    return IP


def get_timestamp():  # 获取当前时间戳
    current_time = datetime.now().strftime('%Y-%m-%dT%H:%M:%S+08:00')
    return current_time


def get_time():  # 获取本机时间
    current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3]
    return current_time


def update_headerId(data):
    data += 1
    if data > 0xFFFFFFFF:
        data = 0
    return data


def angle_rectify(angle):
    rectify = (angle + 180) % 360 - 180
    return rectify


def floatToList(float_value):  # 小数转字符列表
    # 将浮点数转换为二进制表示
    # binary = struct.pack('!f', float_value)#参数'!f'表示使用大端字节序
    binary = struct.pack('f', float_value)
    # 将二进制表示转换为十六进
    hex_array = [int(b) for b in binary]  # int整数表示  hex字符表示
    return hex_array


def listToFloat(list):  # 字符列表转小数
    return round(struct.unpack('<f', struct.pack('4B', *list))[0],
                 5)  # <表示小端字节序 f：这指定了要解析的数据类型为单精度浮点数（float）'4'：这表示接下来的格式字符将被重复四次。'B'：这是单个格式字符，表示无符号字符（unsigned char）。


def SumCheck(data, length):  # 按字节计算校验和
    checksum = 0
    for i in range(0, length):
        checksum += data[i]
        checksum &= 0xFF  # 强制截断
    return checksum


def get_exe_directory():
    # 检查是否是被打包后的环境
    if getattr(sys, 'frozen', False):
        # 如果是被打包后的环境，使用 sys.executable 获取可执行文件路径
        exe_path = sys.executable
    else:
        # 如果不是被打包后的环境，使用 __file__ 获取脚本路径
        exe_path = os.path.abspath(__file__)

    # 获取可执行文件所在的目录
    exe_directory = os.path.dirname(exe_path)
    return exe_directory


class thread_testTime(QThread):

    def __init__(self):
        super().__init__()

    def run(self):  # 该类模拟后台进程更新时间
        while True:
            time.sleep(0.01)
            print(get_time())


# endregion
# region # 自定义类
class DeviceStatusChecker(QWidget):  # 设备离线监测
    update_deviceStatus = pyqtSignal(str)
    sig_reconnect = pyqtSignal(str)

    def __init__(self):
        super().__init__()
        self.status_handler = 0  # 0: 离线 1: 在线 2: 重连
        self.status_chasis = 0
        self.status_lifter = 0
        self.status_motor = 0

        current_time = QTime.currentTime()
        self.time_handler = current_time
        self.time_chasis = current_time
        self.time_lifter = current_time
        self.time_motor = current_time

        self.heartbeat_timer = QTimer()
        self.heartbeat_timer.timeout.connect(self.check_heartbeat)
        self.heartbeat_timer.start(1000)  # 1秒检测一次

    def check_heartbeat(self):
        current_time = QTime.currentTime()
        interval_time_chasis = self.time_chasis.msecsTo(current_time)
        interval_time_lifter = self.time_lifter.msecsTo(current_time)

        if interval_time_chasis < 3000:
            self.status_chasis = 1
        else:
            self.sig_reconnect.emit('chasis')
            self.status_chasis = 2
            self.time_chasis = current_time
            # print('重连')
        if interval_time_lifter < 3000:
            self.status_lifter = 1
        else:
            self.sig_reconnect.emit('lifter')
            self.status_lifter = 2
            self.time_lifter = current_time


class WebControl(QThread):
    app = Flask(__name__)

    def __init__(self):
        super().__init__()

    def run(self):
        self.app.run(host='0.0.0.0', port=5003, debug=True)

    @app.route('/')
    def index():
        return render_template('index.html')

    @app.route('/stop', methods=['POST'])
    def stop_task():
        # 假设这里有一个任务正在运行，我们停止它
        # 但在这个示例中，我们只是简单地返回一个响应
        print('任务已停止')
        return '任务已停止'


class SignalEmitter(QObject):
    flaskWeb_signal = pyqtSignal(str)  # 假设我们传递一个字符串作为消息


class MotorDrive(QThread):
    signal_send_canData = pyqtSignal(bytes)

    def __init__(self):
        super().__init__()
        # self.activate_can_interface('can0')
        if controller == "jihecheng":
            print("jihecheng")
            # self.bus = can.interface.Bus(bustype='canalystii', channel='can0', bitrate=500000)
        elif os.name == 'posix' and virtual_machine == False:
            self.bus = can.interface.Bus(
                interface='socketcan', channel='can0', bitrate=500000)
        # self.configMotor(0x01)
        # self.motorType = "zhongling"  # 中菱电机
        self.motorType = "UsbToCAN"  # UsbToCAN
        self.scale_rateToLine = 0.618  # 角速度到线速度的转换系数
        self.factor_motorLinearSpeed = 100  # 线速度因子
        self.factor_motorangularSpeed = 23.42  # 角速度因子

    def run(self):
        while True:
            time.sleep(1)
            # print(get_time())
            # try:
            #     received_msg = self.bus.recv()
            #     print(f"Received CAN message with device_type {received_msg.arbitration_id} and data {received_msg.data}")
            # except KeyboardInterrupt:
            #     break

    def activate_can_interface(self, interface_name):
        # 构建命令
        command = f"sudo ip link set {interface_name} up type can bitrate 500000"
        # 执行命令
        subprocess.run(command, shell=True, check=True,
                       stdout=subprocess.PIPE, stderr=subprocess.PIPE)
        # result = subprocess.run(command, shell=True, check=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE)
        # 检查是否有错误输出
        # if result.stderr:
        #     print(f"Error activating CAN interface {interface_name}: {result.stderr.decode()}")
        # else:
        #     print(f"CAN interface {interface_name} activated successfully.")

    def control_motor(self, linearSpeed, angularSpeed):
        if self.motorType == "UsbToCAN":
            # print('UsbToCAN')
            print(linearSpeed, angularSpeed)
        else:
            leftMotorSpeed = linearSpeed * self.factor_motorLinearSpeed + \
                angularSpeed * self.factor_motorangularSpeed
            rightMotorSpeed = linearSpeed * self.factor_motorLinearSpeed - \
                angularSpeed * self.factor_motorangularSpeed
            # print(get_time(),linearSpeed)
            # print(get_time())
            speed1 = -int(leftMotorSpeed)  # 左轮安装方向与车体相反，所以取反
            speed2 = int(rightMotorSpeed)
            if self.motorType == "zhongling":
                # 中菱电机
                buf_motor1_speed = [0x23, 0xFF, 0x60, 0x01, speed1 & 0xFF,
                                    (speed1 >> 8) & 0xFF, (speed1 >> 16) & 0xFF, (speed1 >> 24) & 0xFF]
                buf_motor2_speed = [0x23, 0xFF, 0x60, 0x02, speed2 & 0xFF,
                                    (speed2 >> 8) & 0xFF, (speed2 >> 16) & 0xFF, (speed2 >> 24) & 0xFF]
                frame_id = 0x600 | 0x01
                try:
                    self.canSend(frame_id, buf_motor1_speed)
                    self.canSend(frame_id, buf_motor2_speed)
                except Exception as e:
                    print("Error sending CAN message:", e)
            elif self.motorType == "nideke":
                # 尼德科电机
                buf_motor1_speed = [0x23, 0xFF, 0x60, 0x01, speed1 & 0xFF,
                                    (speed1 >> 8) & 0xFF, (speed1 >> 16) & 0xFF, (speed1 >> 24) & 0xFF]
                buf_motor2_speed = [0x23, 0xFF, 0x60, 0x02, speed2 & 0xFF,
                                    (speed2 >> 8) & 0xFF, (speed2 >> 16) & 0xFF, (speed2 >> 24) & 0xFF]
                frame_id = 0x600 | 0x01
                try:
                    self.canSend(frame_id, buf_motor1_speed)
                    self.canSend(frame_id, buf_motor2_speed)
                except Exception as e:
                    print("Error sending CAN message:", e)
            else:
                # print("motor_ZL is not defined.")
                pass
                # 其他电机

    def canSend(self, can_id, data):
        msg = can.Message(arbitration_id=can_id,
                          data=data, is_extended_id=False)
        try:
            if controller == "jihecheng":
                self.signal_send_canData.emit(bytes(data))
            elif os.name == 'posix' and virtual_machine == False:
                self.bus.send(msg)
        except Exception as e:
            print("Error sending CAN message:", e)

    def configMotor(self, motorID):
        # 定义数组
        buf_motor1_active = [0x10, 0x10]
        buf_motor1_setMode = [0x2F, 0x60, 0x60, 0x00, 0xfd, 0x00, 0x00, 0x00]
        buf_motor1_clearError = [0x2B, 0x40,
                                 0x60, 0x00, 0x80, 0x00, 0x00, 0x00]
        buf_motor1_prepareServe = [0x2B, 0x40,
                                   0x60, 0x00, 0x06, 0x00, 0x00, 0x00]
        buf_motor1_waitEn = [0x2B, 0x40, 0x60, 0x00, 0x07, 0x00, 0x00, 0x00]
        buf_motor1_en = [0x2B, 0x40, 0x60, 0x00, 0x0F, 0x00, 0x00, 0x00]
        buf_motor1_setSpeed = [0x23, 0xFF, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00]
        buf_motor1_heartbeat = [0x2B, 0x17, 0x10, 0x00, 0xE8, 0x03, 0x00, 0x00]
        buf_motor1_Watchdog = [0x23, 0x16, 0x10, 0x01, 0x64, 0x00, 0x01, 0x00]
        buf_motor1_protectTime = [0x2B, 0x0C,
                                  0x10, 0x00, 0x64, 0x00, 0x00, 0x00]
        buf_motor1_protectFactor = [0x2F, 0x0D,
                                    0x10, 0x00, 0x01, 0x00, 0x00, 0x00]
        buf_motor_timeoutFactor = [0x2F, 0x2E,
                                   0x20, 0x00, 0x02, 0x00, 0x00, 0x00]

        buf_PDO1_forbid = [0x23, 0x00, 0x18, 0x01, 0x82, 0x01, 0x00, 0x80]
        buf_PDO1_mode = [0x2F, 0x00, 0x18, 0x02, 0xFF, 0x00, 0x00, 0x00]
        buf_PDO1_forbidTime = [0x2B, 0x00, 0x18, 0x03, 0x10, 0x27, 0x00, 0x00]
        buf_PDO1_time = [0x2B, 0x00, 0x18, 0x05, 0xE8, 0x03, 0x00, 0x00]
        buf_PDO1_clear = [0x2F, 0x00, 0x1A, 0x00, 0x00, 0x00, 0x00, 0x00]
        buf_PDO1_address_1 = [0x23, 0x00, 0x1A, 0x01, 0x10, 0x00, 0x01, 0x26]
        buf_PDO1_address_2 = [0x23, 0x00, 0x1A, 0x02, 0x10, 0x00, 0x02, 0x26]
        buf_PDO1_numer = [0x2F, 0x00, 0x1A, 0x00, 0x02, 0x00, 0x00, 0x00]
        buf_PDO1_enable = [0x23, 0x00, 0x18, 0x01, 0x82, 0x01, 0x00, 0x00]

        buf_PDO2_forbid = [0x23, 0x01, 0x18, 0x01, 0x83, 0x02, 0x00, 0x80]
        buf_PDO2_mode = [0x2F, 0x01, 0x18, 0x02, 0xFF, 0x00, 0x00, 0x00]
        buf_PDO2_forbidTime = [0x2B, 0x01, 0x18, 0x03, 0x10, 0x27, 0x00, 0x00]
        buf_PDO2_time = [0x2B, 0x01, 0x18, 0x05, 0xE8, 0x03, 0x00, 0x00]
        buf_PDO2_clear = [0x2F, 0x01, 0x1A, 0x00, 0x00, 0x00, 0x00, 0x00]
        buf_PDO2_address_1 = [0x23, 0x01, 0x1A, 0x01, 0x20, 0x00, 0x63, 0x60]
        buf_PDO2_address_2 = [0x23, 0x01, 0x1A, 0x02, 0x20, 0x00, 0x6C, 0x60]
        buf_PDO2_numer = [0x2F, 0x01, 0x1A, 0x00, 0x02, 0x00, 0x00, 0x00]
        buf_PDO2_enable = [0x23, 0x01, 0x18, 0x01, 0x83, 0x02, 0x00, 0x00]

        frameID = 0x600 | motorID
        DELAY_TIME = 0.02
        time.sleep(DELAY_TIME)

        buf_PDO1_forbid[4] = 0x80 | motorID
        buf_PDO1_enable[4] = 0x80 | motorID
        buf_PDO2_forbid[4] = 0x80 | motorID
        buf_PDO2_enable[4] = 0x80 | motorID

        self.canSend(frameID, buf_motor_timeoutFactor)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_motor1_protectTime)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_motor1_protectFactor)
        time.sleep(DELAY_TIME)

        self.canSend(frameID, buf_PDO1_forbid)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO1_mode)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO1_forbidTime)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO1_time)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO1_clear)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO1_address_1)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO1_address_2)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO1_numer)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO1_enable)
        time.sleep(DELAY_TIME)

        self.canSend(frameID, buf_PDO2_forbid)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO2_mode)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO2_forbidTime)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO2_time)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO2_clear)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO2_address_1)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO2_address_2)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO2_numer)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_PDO2_enable)
        time.sleep(DELAY_TIME)

        self.canSend(frameID, buf_motor1_setMode)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_motor1_clearError)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_motor1_prepareServe)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_motor1_waitEn)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_motor1_en)
        time.sleep(DELAY_TIME)
        self.canSend(frameID, buf_motor1_setSpeed)
        time.sleep(DELAY_TIME)

        print(f"电机{motorID:02x}配置完成;")


class PGV(QThread):
    sig_PGV = pyqtSignal(int, int, int, int)
    sig_updateOdom = pyqtSignal(float, float, float)

    def __init__(self):
        super().__init__()
        # region # 串口初始化
        self.ser = serial.Serial()
        if controller == "jihecheng":
            self.ser.port = '/dev/ttyUSB0'
        else:
            self.ser.port = '/dev/ttyS4'
        # self.ser.port = 'COM8'
        self.ser.baudrate = 115200  # 波特率
        self.ser.timeout = 0.007  # 超时时间（115200）
        self.ser.bytesize = 8  # 数据位
        self.ser.parity = 'N'  # 校验位
        self.ser.stopbits = 1  # 停止位
        self.query = QTimer()
        self.query.timeout.connect(self.fuc_query)
        self.query.start(100)
        self.flag_serial = False
        # endregion
        # region # 二维码数据初始化
        if os.name == 'nt':  # Windows系统
            os.chdir(r'\Code\mqtt_client')
            fileName = './config/DmCodes.xlsx'
        elif controller == "jihecheng":
            fileName = '/home/robot/Desktop/theyn/mqtt_client/config/DmCodes.xlsx'
        elif virtual_machine == True:  # 虚拟机
            print(os.getcwd())
            print("虚拟机,Linux系统")
            fileName = '/home/theyn/Desktop/mqtt_client/config/DmCodes.xlsx'
        elif os.name == 'posix' and virtual_machine == False:  # Linux系统
            print("Linux系统")
            fileName = '/home/baer/mqtt_client/config/DmCodes.xlsx'
        df = pd.read_excel(fileName)
        array = df.values  # 或者使用 df.to_numpy()
        columns = ['序号', 'tag值', 'X(mm)', 'Y(mm)', '误差角度(°)']
        rows_dict = [dict(zip(columns, row))
                     for row in array]  # 将每行数据转换为字典，并存入列表
        self.dmCodes = {row['tag值']: row for row in rows_dict}  # 创建以tag值为键的字典
        # endregion

    def run(self):
        # try:
        if controller == "jihecheng":
            try:
                self.ser.open()
                self.flag_serial = True
            except serial.SerialException:
                print('相机未连接')
        elif os.name == 'posix' and virtual_machine == False:
            self.ser.open()
            self.flag_serial = True
        # except Exception as e:
        #     print(e)
        #     print('此串口不能正常打开！')
        #     self.flag_serial = False
        while True:
            # while False:
            try:
                if self.ser.in_waiting:
                    data = self.ser.readall()  # 不能接收持续发送的数据
                    if not data or data == 'exit':
                        print("break")
                        break
                    # self.signal.data_process.emit(data)
                    # print(len(data))
                    if len(data) == 21:
                        self.analysisPGV(data)
            except Exception:
                if self.flag_serial:
                    print("串口接收数据异常")
                self.flag_serial = False
                time.sleep(1)
                try:
                    if controller == "jihecheng":
                        self.ser.open()
                        self.flag_serial = True
                    elif os.name == 'posix' and virtual_machine == False:
                        self.ser.open()
                        self.flag_serial = True
                except serial.SerialException:
                    # print('相机串口连接异常！')
                    pass

    def analysisPGV(self, i_PGV_Data):
        # 初始化变量
        flag_tagChange = 0
        tmp_posX = 0
        tmp_tagNum = 0
        tmp_posY = 0
        tmp_posYaw = 0
        tmp_agv_imuAngle = 0.0

        # posX
        if i_PGV_Data[2] & 0x04:
            # posX是负数
            tmp_posX = (0xffffff - ((i_PGV_Data[5] & 0x7f) | ((i_PGV_Data[4] & 0x7f) << 7) | ((i_PGV_Data[3] & 0x7f) << 14) |
                                    ((i_PGV_Data[2] & 0x07) << 21))) * -1
        else:
            # posX是正数
            tmp_posX = (i_PGV_Data[5] & 0x7f) | ((i_PGV_Data[4] & 0x7f) << 7) | (
                (i_PGV_Data[3] & 0x7f) << 14) | ((i_PGV_Data[2] & 0x03) << 21)

        # posY
        if i_PGV_Data[6] & 0x40:
            # posY是负数
            tmp_posY = (
                (~((i_PGV_Data[7] & 0x7f) | ((i_PGV_Data[6] & 0x7f) << 7)) + 1) & 0x3fff) * -1
        else:
            # posY是正数
            tmp_posY = (i_PGV_Data[7] & 0x7f) | ((i_PGV_Data[6] & 0x7f) << 7)

        # posYAW
        if i_PGV_Data[10] & 0x40:
            # posYAW是负数
            tmp_posYaw = ((~((i_PGV_Data[11] & 0x7f) | (
                (i_PGV_Data[10] & 0x7f) << 7)) + 1) & 0x3fff) * -1
        else:
            # posYAW是正数
            tmp_posYaw = (i_PGV_Data[11] & 0x7f) | (
                (i_PGV_Data[10] & 0x7f) << 7)

        tmp_posYaw = 3600 - tmp_posYaw
        tmp_tagNum = (i_PGV_Data[17] & 0x7f) | ((i_PGV_Data[16] & 0x7f) << 7) | (
            (i_PGV_Data[15] & 0x7f) << 14) | ((i_PGV_Data[14] & 0x7f) << 21)

        # 打印调试信息
        # print(f"X位置: {tmp_posX}, Y位置: {tmp_posY}, 角度: {tmp_posYaw}, 编码: {tmp_tagNum}")
        self.sig_PGV.emit(tmp_tagNum, tmp_posX, tmp_posY, tmp_posYaw)
        if tmp_tagNum:
            matched_data = self.dmCodes.get(tmp_tagNum)  # 匹配tag值
            if matched_data:
                posX = (matched_data['X(mm)'] + tmp_posX / 10) / 1000
                posY = (matched_data['Y(mm)'] + tmp_posY / 10) / 1000
                posYaw = (matched_data['误差角度(°)'] + tmp_posYaw / 10)
                # print(f"X位置: {posX}, Y位置: {posY}, 角度: {tmp_posYaw}, 编码: {tmp_tagNum}")
                self.sig_updateOdom.emit(posX, posY, posYaw)
            else:
                print("匹配失败")

    def fuc_query(self):
        buf_PGV_query = bytes([0xC8, 0x37])
        if self.flag_serial:
            self.ser.write(buf_PGV_query)


class Odometer(QThread):
    # region # sigal
    sig_updateAgvPos = pyqtSignal(float, float, float)
    sig_updateAgvSpeed = pyqtSignal(float, float)
    sig_updateQrcode = pyqtSignal(int, float)
    sig_updateStandly = pyqtSignal(float, float, float)
    sig_updateMotionDone = pyqtSignal()
    # endregion

    def __init__(self):
        super().__init__()
        self.control_mode = 'mannul'
        # region # 里程计相关
        self.agv_posX = AgvState['agvPosition']['x']
        self.agv_posY = AgvState['agvPosition']['y']
        self.agv_theta = AgvState['agvPosition']['theta']
        self.agv_angle = self.agv_theta * 180 / math.pi
        self.linearSpeed = 0
        self.angularSpeed = 0
        self.multiLinearSpeed = 1  # 线速度倍率(提高模拟效率)
        self.multiangularSpeed = 1  # 角速度倍率(提高模拟效率)
        self.start_time = time.time()
        # self.execution_time = 0
        self.mode_motion = 'stop'  # 'stop' 'move' 'moveToNode' 'rotate' 'rotateToNode'
        self.startNode_x = 0  # 起始点x
        self.startNode_y = 0  # 起始点y
        self.targetNode_x = 0  # 目标点x
        self.targetNode_y = 0  # 目标点y
        self.planDistance = 0  # 距离目标点距离
        self.decDistance = 0  # 提前减速距离
        self.flag_slowDown = False  # 是否开始减速
        self.flag_enSlowDown = True  # 是否需要减速
        self.creepLinearSpeed = 0.03  # 爬行线速度
        self.creepDistance = 0.03  # 爬行距离

        self.rotateTheta_T = 0  # 目标角度
        self.planTheta_last = 0  # 上次距离目标点角度
        self.decTheta = 0  # 提前减速角度
        self.creepangularSpeed = 0.03  # 爬行角速度
        self.creepTheta = 0.03  # 爬行角度
        self.flag_simulate = False  # 模拟模式
        # endregion
        # region # 速度曲线相关
        self.MotorDriveType = "CAN"
        self.flag_controlSpeed = 0
        self.linearSpeed_T = 0
        self.linearSpeed_C = 0
        self.linearSpeed_T_last = 0
        self.agv_max_linearSpeed = 1.2
        self.factor_sysDelay = 1  # python代码执行效率低，定时10ms循环执行只能到30-50ms，提高AGV模拟器加减速度
        self.agv_acceleration = 0.7 * self.multiLinearSpeed * self.factor_sysDelay
        self.agv_deceleration = 0.7 * self.multiLinearSpeed * self.factor_sysDelay
        self.angularSpeed_T = 0
        self.angularSpeed_C = 0
        self.angularSpeed_T_last = 0
        self.agv_max_angularSpeed = 1.2
        self.agv_RateAcceleration = 0.7 * self.multiangularSpeed * self.factor_sysDelay
        self.agv_RateDeceleration = 0.7 * self.multiangularSpeed * self.factor_sysDelay
        self.time_period = 20
        self.time_interval = self.time_period / 1000
        # endregion
        # region # IMU修正
        self.imu_angle = 0
        self.imu_offset = 0
        # endregion
        # region # standly纠偏

        self.K_Cross = 0  # 横向误差系数
        self.K_LowVelo = 0  # 低速阻尼
        self.K_trace_angle = 0  # 轨迹纠偏系数
        self.K_cross_angle = 0  # 航向角纠偏系数
        self.path_direction = 0  # 路径方向
        self.flag_enStandly = True  # 路径方向
        self.heading_angle = 0  # 航向角
        self.flag_car_backward = False  # 是否倒车
        # endregion

    def run(self):
        while True:
            start_time = time.time()
            self.odometer()
            self.speedControl()
            end_time = time.time()
            execution_time = end_time - start_time
            sleep_time = self.time_interval - 0.000 - \
                execution_time  # 理论time_interval 实际测试-0.001
            if sleep_time > 0:
                time.sleep(sleep_time)

    def odometer(self):  # 里程计
        if self.angularSpeed != 0 or self.linearSpeed != 0:
            # region # 更新AGV位置
            if self.angularSpeed != 0 and self.flag_simulate:
                div_theta = self.angularSpeed * self.time_interval * self.multiangularSpeed
                # 里程计计算角度
                self.agv_theta += div_theta
                # 确保偏航角差在[-pi, pi]范围内
                self.agv_theta = (
                    self.agv_theta + math.pi) % (2 * math.pi) - math.pi
                self.agv_angle = self.agv_theta * 180 / math.pi
            if self.linearSpeed != 0:
                div_linearSpeed = self.linearSpeed * self.time_interval * self.multiLinearSpeed
                self.agv_posX += div_linearSpeed * math.cos(self.agv_theta)
                self.agv_posY += div_linearSpeed * math.sin(self.agv_theta)
            self.sig_updateAgvPos.emit(
                self.agv_posX, self.agv_posY, self.agv_theta)
            # endregion
            if self.mode_motion == 'moveToNode':
                x1 = self.agv_posX
                y1 = self.agv_posY
                x2 = self.targetNode_x
                y2 = self.targetNode_y
                x3 = self.startNode_x
                y3 = self.startNode_y
                A = (x2, y2)  # 终点
                B = (x1, y1)  # 当前
                C = (x3, y3)  # 起点

                # 横向距离
                distance_direction, distance_cross = self.projection_distance(
                    A, B, C)  # 投影距离,横向距离
                distance_cross = -distance_cross
                if abs(distance_cross) > DeviceParameters.max_distance_cross:
                    self.securityProtect(1)
                else:
                    # 横向纠偏角
                    if distance_direction < 0.05:
                        angle_cross = 0
                    else:
                        angle_cross = math.atan2(
                            self.K_Cross * distance_cross, abs(self.linearSpeed) + self.K_LowVelo)
                    # 航向角
                    if self.flag_car_backward:
                        self.heading_angle = self.agv_angle + 180
                    else:
                        self.heading_angle = self.agv_angle
                    self.heading_angle = angle_rectify(self.heading_angle)
                    # 航向角偏差
                    trace_angle_deviation = self.path_direction - self.heading_angle
                    trace_angle_deviation = angle_rectify(
                        trace_angle_deviation)
                    if abs(trace_angle_deviation) > DeviceParameters.max_trace_angle_deviation:
                        self.securityProtect(2)
                    else:
                        # 纠偏角速度(弧度/秒)
                        self.angularSpeed_T = self.K_trace_angle * trace_angle_deviation / \
                            180 * math.pi + self.K_cross_angle * angle_cross
                        if self.flag_enStandly == False:
                            self.angularSpeed_T = 0

                        self.sig_updateStandly.emit(
                            round(distance_cross, 3)*1000, round(trace_angle_deviation, 3), round(self.angularSpeed_T, 3))
                self.flag_controlSpeed = 1

                # 到达目标点
                if distance_direction < 0.005:
                    print(
                        f'到达目标点,本次距离{distance_direction:.3f}m,当前速度{self.linearSpeed:.3f}m/s')
                    if self.flag_enSlowDown:
                        self.linearSpeed_T = 0
                        self.angularSpeed_T = 0
                        self.flag_controlSpeed = 1
                    self.sig_updateMotionDone.emit()
                    self.mode_motion = 'stop'
                    self.flag_slowDown = False
                # 接近目标点
                elif distance_direction < self.decDistance and self.flag_slowDown == False and self.flag_enSlowDown:
                    print(f'开始减速{distance_direction:.3f}m')
                    if self.linearSpeed >= 0:
                        self.linearSpeed_T = self.creepLinearSpeed
                    else:
                        self.linearSpeed_T = -self.creepLinearSpeed

                    self.angularSpeed_T = 0
                    self.flag_controlSpeed = 1
                    self.flag_slowDown = True
            elif self.mode_motion == 'rotateToNode':
                planTheta = abs(self.calculate_theta_deviation(
                    self.agv_theta, self.rotateTheta_T))
                # 到达目标角
                if planTheta < 0.005 or planTheta - self.planTheta_last > 0.001:
                    if planTheta < 0.005:
                        print(
                            f'到达目标角度,本次偏差弧度{planTheta:.3f},当前速度{self.angularSpeed:.3f}')
                    else:
                        print(
                            f'到达目标角度,本次偏差角度{planTheta:.3f},上次偏差角度{self.planTheta_last:.3f}m')
                    self.linearSpeed_T = 0
                    self.angularSpeed_T = 0
                    self.flag_controlSpeed = 1
                    self.sig_updateMotionDone.emit()
                    self.mode_motion = 'stop'
                    self.flag_slowDown = False
                # 接近目标角
                elif planTheta < self.decTheta and self.flag_slowDown == False:
                    print(f'开始减速{planTheta:.3f}')
                    self.linearSpeed_T = 0
                    if self.angularSpeed >= 0:
                        self.angularSpeed_T = self.creepangularSpeed
                    else:
                        self.angularSpeed_T = -self.creepangularSpeed
                    self.flag_controlSpeed = 1
                    self.flag_slowDown = True
                self.planTheta_last = planTheta

    def speedControl(self):  # 速度曲线
        if self.flag_controlSpeed:
            # region # 线速度曲线控制
            self.linearSpeed_T = max(
                min(self.linearSpeed_T, self.agv_max_linearSpeed), -self.agv_max_linearSpeed)
            if self.linearSpeed_C == self.linearSpeed_T:
                pass
            elif (self.linearSpeed_C < self.linearSpeed_T) and (self.linearSpeed_C >= 0) and (self.linearSpeed_T >= 0):
                # 当前速度和目标速度同正,正向加速
                self.linearSpeed_C += self.agv_acceleration * self.time_period / 1000
                if self.linearSpeed_C > self.linearSpeed_T:
                    self.linearSpeed_C = self.linearSpeed_T
            elif (self.linearSpeed_C > self.linearSpeed_T) and (self.linearSpeed_C >= 0) and (self.linearSpeed_T >= 0):
                # 当前速度和目标速度同正,正向减速
                self.linearSpeed_C -= self.agv_deceleration * self.time_period / 1000
                if self.linearSpeed_C < self.linearSpeed_T:
                    self.linearSpeed_C = self.linearSpeed_T
            elif (self.linearSpeed_C > self.linearSpeed_T) and (self.linearSpeed_C <= 0) and (self.linearSpeed_T <= 0):
                # 当前速度和目标速度同负,反向加速
                self.linearSpeed_C -= self.agv_acceleration * self.time_period / 1000
                if self.linearSpeed_C < self.linearSpeed_T:
                    self.linearSpeed_C = self.linearSpeed_T
            elif (self.linearSpeed_C < self.linearSpeed_T) and (self.linearSpeed_C <= 0) and (self.linearSpeed_T <= 0):
                # 当前速度和目标速度同负,反向减速
                self.linearSpeed_C += self.agv_deceleration * self.time_period / 1000
                if self.linearSpeed_C > self.linearSpeed_T:
                    self.linearSpeed_C = self.linearSpeed_T
            elif (self.linearSpeed_C < 0) and (self.linearSpeed_T > 0):
                # 当前速度负，目标速度正,先反向减速
                self.linearSpeed_C += self.agv_deceleration * self.time_period / 1000
            elif (self.linearSpeed_C > 0) and (self.linearSpeed_T < 0):
                # 当前速度正，目标速度负,先正向减速
                self.linearSpeed_C -= self.agv_deceleration * self.time_period / 1000
            else:
                print("线速度数据错误;")
            # endregion
            # region # 角速度曲线控制
            self.angularSpeed_T = max(
                min(self.angularSpeed_T, self.agv_max_angularSpeed), -self.agv_max_angularSpeed)
            if self.angularSpeed_C == self.angularSpeed_T:
                pass
            elif (self.angularSpeed_C < self.angularSpeed_T) and (self.angularSpeed_C >= 0) and (self.angularSpeed_T >= 0):
                # 当前速度和目标速度同正,正向加速
                self.angularSpeed_C += self.agv_RateAcceleration * self.time_period / 1000
                if self.angularSpeed_C > self.angularSpeed_T:
                    self.angularSpeed_C = self.angularSpeed_T
            elif (self.angularSpeed_C > self.angularSpeed_T) and (self.angularSpeed_C >= 0) and (self.angularSpeed_T >= 0):
                # 当前速度和目标速度同正,正向减速
                self.angularSpeed_C -= self.agv_RateDeceleration * self.time_period / 1000
                if self.angularSpeed_C < self.angularSpeed_T:
                    self.angularSpeed_C = self.angularSpeed_T
            elif (self.angularSpeed_C > self.angularSpeed_T) and (self.angularSpeed_C <= 0) and (self.angularSpeed_T <= 0):
                # 当前速度和目标速度同负,反向加速
                self.angularSpeed_C -= self.agv_RateAcceleration * self.time_period / 1000
                if self.angularSpeed_C < self.angularSpeed_T:
                    self.angularSpeed_C = self.angularSpeed_T
            elif (self.angularSpeed_C < self.angularSpeed_T) and (self.angularSpeed_C <= 0) and (self.angularSpeed_T <= 0):
                # 当前速度和目标速度同负,反向减速
                self.angularSpeed_C += self.agv_RateDeceleration * self.time_period / 1000
                if self.angularSpeed_C > self.angularSpeed_T:
                    self.angularSpeed_C = self.angularSpeed_T
            elif (self.angularSpeed_C < 0) and (self.angularSpeed_T > 0):
                # 当前速度负，目标速度正,先反向减速
                self.angularSpeed_C += self.agv_RateDeceleration * self.time_period / 1000
            elif (self.angularSpeed_C > 0) and (self.angularSpeed_T < 0):
                # 当前速度正，目标速度负,先正向减速
                self.angularSpeed_C -= self.agv_RateDeceleration * self.time_period / 1000
            else:
                print("角速度数据错误;")
            # endregion
            if self.linearSpeed_C == self.linearSpeed_T and self.angularSpeed_C == self.angularSpeed_T:
                self.flag_controlSpeed = 0
            # region # 速度下发到电机
            if self.MotorDriveType == "CAN":  # 一直发
                self.sig_updateAgvSpeed.emit(
                    self.linearSpeed_C, self.angularSpeed_C)
            elif self.MotorDriveType == "UsbToCAN":  # 变化时发
                if self.linearSpeed_T != self.linearSpeed_T_last or self.angularSpeed_T != self.angularSpeed_T_last:
                    self.sig_updateAgvSpeed.emit(
                        self.linearSpeed_T, self.angularSpeed_T)
                    self.linearSpeed_T_last = self.linearSpeed_T
                    self.angularSpeed_T_last = self.angularSpeed_T
            self.linearSpeed = self.linearSpeed_C
            self.angularSpeed = self.angularSpeed_C
            # endregion

    def projection_distance(self, A, B, C):  # 目标A，当前B，起点C；计算AB在AC的投影长度
        # 提取点的坐标
        xA, yA = A
        xB, yB = B
        xC, yC = C

        # 计算向量AB和AC
        AB = (xB - xA, yB - yA)
        AC = (xC - xA, yC - yA)

        # 计算点积
        dot_product = AB[0] * AC[0] + AB[1] * AC[1]

        # 计算向量AC的模
        magnitude_AC = math.sqrt(AC[0]**2 + AC[1]**2)

        # 计算叉积
        cross_product = AB[0] * AC[1] - AB[1] * AC[0]

        # 计算向量AC的模
        magnitude_AC = math.sqrt(AC[0]**2 + AC[1]**2)

        # 计算投影距离
        projection_dist = dot_product / magnitude_AC

        # 计算点B到向量AC的距离
        cross_distance = cross_product / magnitude_AC

        return projection_dist, cross_distance

    def calculate_distance(self, x1, y1, x2, y2):
        distance = math.sqrt((x2 - x1)**2 + (y2 - y1)**2)
        return distance

    def calculate_theta(self, x1, y1, x2, y2):
        theta = math.atan2((y2 - y1), (x2 - x1))
        return theta

    def calculate_theta_deviation(self, theta_C, theta_T):
        theta = theta_T - theta_C
        theta = (theta + math.pi) % (2 * math.pi) - math.pi
        return theta

    def updateOdom_PGV(self, x, y, angle, tagnum):
        deviation_x = self.agv_posX - x
        deviation_y = self.agv_posY - y
        # 沿Y轴运动修正角度
        if (self.path_direction > 80 and self.path_direction < 100):
            if abs(deviation_x) > 0.01:
                self.sig_updateQrcode.emit(tagnum, deviation_x)
            if abs(deviation_y) > 0.05:
                print(
                    f'dedeviation_x{round(deviation_x,3)},deviation_y{round(deviation_y,3)},tmp_tagNum{tagnum}')
        elif self.path_direction > -100 and self.path_direction < -80:
            if abs(deviation_x) > 0.01:
                self.sig_updateQrcode.emit(tagnum, -deviation_x)
            if abs(deviation_y) > 0.05:
                print(
                    f'dedeviation_x{round(deviation_x,3)},deviation_y{round(deviation_y,3)},tmp_tagNum{tagnum}')
        self.agv_posX = x
        self.agv_posY = y
        if angle > 180:
            angle -= 360
        elif angle < -180:
            angle += 360
        self.agv_angle = angle
        self.imu_offset = self.agv_angle-self.imu_angle

    def updateIMU(self, angle):
        self.imu_angle = -angle
        self.agv_angle = self.imu_angle + self.imu_offset
        self.agv_theta = self.agv_angle / 180 * math.pi
        self.agv_theta = (
            self.agv_theta + math.pi) % (2 * math.pi) - math.pi
        # print("IMU角度:",self.imu_angle)

    def securityProtect(self, num):
        if self.control_mode != 'mannul':  # 只有在自动模式下才进行安全保护
            print(f"安全保护{num}")
            self.linearSpeed_T = 0
            self.angularSpeed_T = 0
            if num == 2:
                print(f'路径方向{self.path_direction:.3f}°')
                print(f'AGV角度{self.agv_theta:.3f}°')
                print(f'AGV角度{self.agv_angle:.3f}°')
                print(f'航向角{self.heading_angle:.3f}°')


class MqttThread(QThread):
    sig_connect_feedback = pyqtSignal(bool)
    server_log = pyqtSignal(str)
    sig_data_analysis = pyqtSignal(str, bytes)
    signal_send_data = pyqtSignal(bytes)
    received_data = pyqtSignal(tuple, str)
    update_clients_list = pyqtSignal(str, tuple)
    flaskWeb_signal = pyqtSignal()
    sig_controlDevice_change = pyqtSignal(str)

    def __init__(self, ip, port, device_type):
        super().__init__()
        self.ip = ip
        self.port = port
        self.device_type = device_type
        self.client_sockets = []
        self.controlDevice = ''
        self.signal_send_data.connect(self.send_data)
        self.sig_controlDevice_change.connect(self.controlDevice_change)

    def run(self):
        self.client = mqtt_client.Client(
            mqtt_client.CallbackAPIVersion.VERSION2, self.device_type)
        # print('self.device_type:',self.device_type)
        # self.client = mqtt_client.Client(str(uuid.uuid4()))
        self.client.on_connect = self.on_connect  # 返回连接状态的回调函数
        self.client.on_message = self.on_message  # 返回订阅消息回调函数
        self.client.username_pw_set("theyn", "password")  # mqtt服务器账号密码
        try:
            self.client.connect(self.ip, self.port, 60)  # 连接mqtt服务器
            self.client.loop_start()
            self.flag_run = True
            self.server_log.emit("已连接MQTT服务器")
            self.sig_connect_feedback.emit(True)
            while self.flag_run:
                time.sleep(1)
                # print("服务器正在运行...")
            self.client.disconnect()
            self.server_log.emit("已断开MQTT服务器")
            print("服务器已停止")
        except Exception as e:
            self.server_log.emit("连接失败" + str(e))
            self.sig_connect_feedback.emit(False)

    def stop(self):
        self.flag_run = False

    def send_data(self, data):
        for i in range(0, len(self.client_sockets)):
            try:
                self.client_sockets[i].send(data)
            except:
                self.client_sockets.remove(self.client_sockets[i])
                self.server_log.emit(f"客户端 {self.client_sockets[i]} 已断开连接")

    def handle_client(self, client_socket, client_address):
        client_text = client_address[0] + ':' + str(client_address[1])
        print(f"客户端 {client_text} 已连接")
        while self.flag_run:
            try:
                data = client_socket.recv(1024)
                if not data:
                    break
                if (client_text == self.controlDevice):  # 当前控制设备
                    self.server_log.emit(data)
                else:  # 其他设备
                    try:
                        self.received_data.emit(
                            client_address, data.decode('utf-8'))
                    except Exception as e:
                        print(e)
            except Exception as e:
                print(f"客户端 {client_address} 断开连接，原因： {e}")
                break
        client_socket.close()
        self.server_log.emit(f"客户端 {client_address} 断开连接")
        self.client_sockets.remove(client_socket)
        self.update_clients_list.emit("remove", client_address)  # 更新界面列表

    def controlDevice_change(self, device):
        self.controlDevice = device
        print(self.controlDevice)

    def on_connect(self, client, userdata, flags, reason_code, properties):
        print(f"Connected with result code {reason_code}")
        # 在连接时设置遗嘱
        # 主题（遗嘱将被发送到此主题）
        TOPIC = "Test/Topic1"
        # 遗嘱消息
        lastWill = AgvParameters.lastWill
        lastWill['timestamp'] = get_timestamp()
        LWT_MESSAGE = json.dumps(lastWill)
        # LWT_MESSAGE = f"Client disconnected unexpectedly"
        # 遗嘱的QoS
        LWT_QOS = 1
        # 遗嘱的retain标志
        LWT_RETAIN = True
        self.client.will_set(TOPIC, LWT_MESSAGE, LWT_QOS, LWT_RETAIN)

    def on_message(self, client, userdata, msg):
        # print(f"{msg.topic} {msg.payload}")
        message = msg.topic + ': ' + msg.payload.decode("utf-8")
        self.server_log.emit(message)
        self.sig_data_analysis.emit(msg.topic, msg.payload)

    def subscribe(self, topic):
        self.client.subscribe(topic)
        self.server_log.emit('subscribed to topic ' + topic)

    def unsubscribe(self, topic):
        self.client.unsubscribe(topic)
        self.server_log.emit('unsubscribed from topic ' + topic)

    def publish(self, topic, message, isShow):
        self.client.publish(topic, message)
        if isShow:
            self.server_log.emit('published to topic ' +
                                 topic + ' message ' + message)


class ServerThread(QThread):
    received_data = pyqtSignal(tuple, str)
    server_log = pyqtSignal(str)
    update_clients_list = pyqtSignal(str, tuple)
    stop_signal = pyqtSignal()
    signal_send_data = pyqtSignal(bytes)
    sig_data_process = pyqtSignal(tuple, int, bytes)
    sig_controlDevice_change = pyqtSignal(str)

    def __init__(self, host, port, device_type):
        super().__init__()
        self.host = host
        self.port = port
        self.device_type = device_type
        self.client_sockets = []
        self.controlDevice = ''
        self.signal_send_data.connect(self.send_data)
        self.sig_controlDevice_change.connect(self.controlDevice_change)

    def run(self):
        self.server_socket = socket.socket(
            socket.AF_INET, socket.SOCK_STREAM)
        self.server_socket.setsockopt(
            socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        self.server_socket.bind((self.host, self.port))
        self.server_socket.listen(5)
        self.flag_run = True
        self.server_log.emit("服务器已启动，等待客户端连接...")
        while self.flag_run:
            try:
                client_socket, client_address = self.server_socket.accept()
                # client_socket.settimeout(2)  # 设置超时时间为3秒,3s内无数据传输则断开连接
                
                self.client_sockets.append(client_socket)
                self.update_clients_list.emit("add", client_address)  # 更新界面列表
                print(f"客户端 {client_address} 已连接")
                self.server_log.emit(f"客户端 {client_address} 已连接")
                threading.Thread(target=self.handle_client, args=(
                    client_socket, client_address)).start()
            except:
                self.server_log.emit("服务器已关闭")
                # client_socket.close()

    def stop(self):
        self.flag_run = False
        self.server_socket.close()
        # 关闭所有连接设备
        # for i in range(0,len(self.client_sockets)):
        #     try:
        #         self.client_sockets[i].send(b"exit")
        #         self.client_sockets[i].close()
        #     except:
        #         break
        # self.client_sockets=[]

    def send_data(self, data):
        for i in range(0, len(self.client_sockets)):
            try:
                self.client_sockets[i].send(data)
            except:
                self.client_sockets.remove(self.client_sockets[i])
                self.client_sockets[i].close()
                self.server_log.emit(f"客户端 {self.client_sockets[i]} 已断开连接")

    def handle_client(self, client_socket, client_address):
        while self.flag_run:
            try:
                data = client_socket.recv(1024)
                if not data:
                    break
                else:
                    # print(data)
                    self.sig_data_process.emit(
                        client_address, self.device_type, data)
            except Exception as e:
                print(f"客户端 {client_address} 断开连接，原因： {e}")
                break
        client_socket.close()
        self.server_log.emit(f"客户端 {client_address} 断开连接")
        self.client_sockets.remove(client_socket)
        self.update_clients_list.emit("remove", client_address)  # 更新界面列表

    def controlDevice_change(self, device):
        self.controlDevice = device
        print(self.controlDevice)


class clientThread(QThread):
    # 定义信号来发送接收到的数据
    data_received = pyqtSignal(tuple, int, bytes)
    log = pyqtSignal(str)
    disConnect = pyqtSignal(int, QThread)

    def __init__(self, device_type, server_address):
        super().__init__()
        self.device_type = device_type
        self.server_address = server_address
        try:
            self.identify = server_address.getpeername()
            self.running = True
        except:
            self.identify = server_address
            self.running = False

    def run(self):
        current_thread = threading.current_thread()  # 获取线程对象、名称，便于从线程列表中移除
        sender = self.sender()
        # print(f"Thread {current_thread.name} started")
        # print(sender)
        # print(f"Running in thread: {self}")
        # print(self.device_type)
        while self.running:
            try:
                data = self.server_address.recv(1024)
                if data:
                    self.data_received.emit(
                        self.identify, self.device_type, data)
                else:
                    self.stop()
            except:
                self.stop()

    def stop(self):
        self.log.emit(str(self.identify) + '断开连接')
        self.server_address.close()
        self.running = False

    def send_message(self, message):
        if self.server_address and self.server_address.fileno() != -1:
            try:
                self.server_address.sendall(message)
            except:
                self.stop()
                # self.log.emit(f"Error sending message to {str(self.identify)}")
                # self.log.emit(str(self.identify) + "已断开连接")
                # self.disConnect.emit(self.device_type, self)
                # self.server_address.close()
        else:
            self.log.emit(str(self.identify) + "连接不存在")

            # self.disConnect.emit(self.server_address)


class SerialPort(QThread):
    data_received = pyqtSignal(str, int, bytes)

    def __init__(self, port, baudrate=9600, timeout=0.1, device_type=0):
        """
        初始化串口对象

        :param port: 串口名称，例如 'COM3' 或 '/dev/ttyUSB0'
        :param baudrate: 波特率，默认 9600
        :param timeout: 读取超时时间，默认 1 秒
        """
        super().__init__()
        self.port = port
        self.baudrate = baudrate
        self.timeout = timeout
        self.serial = None
        self.identify = port
        self.device_type = device_type

    def run(self):
        self.open()
        while self.is_open():
            try:
                if self.serial.in_waiting:
                    data = self.serial.read(256)
                    if not data or data == 'exit':
                        print("exit")
                        self.close()
                        break
                    else:
                        # print(data)
                        # print(self.identify)
                        # print(self.device_type)
                        self.data_received.emit(
                            self.identify, self.device_type, data)
            except Exception as e:
                print(f"串口读取错误: {e}")
                self.close()
                break

    def open(self):
        """打开串口"""
        try:
            self.serial = serial.Serial(
                self.port, self.baudrate, timeout=self.timeout)
            # self.serial.reset_input_buffer(512)
            # import termios
            # import fcntl
            # fd = self.serial.fileno()
            # print(fd)
            # buffer_size = 4096  # 设置缓冲区大小为 4096 字节
            # fcntl.ioctl(fd, termios.TIOCSETF, termios.B4096)
            print(f"串口 {self.port} 已打开")
        except serial.SerialException as e:
            print(f"无法打开串口 {self.port}: {e}")

    def is_open(self):
        """检查串口是否已打开"""
        return self.serial and self.serial.is_open

    def close(self):
        """关闭串口"""
        if self.serial and self.serial.is_open:
            self.serial.close()
            print(f"串口 {self.port} 已关闭")
        else:
            print(f"串口 {self.port} 未打开")

    def write(self, data):
        """
        写入数据到串口

        :param data: 要写入的数据，字符串或字节
        """
        if not self.is_open():
            print("串口未打开")
            self.open()
            return

        if isinstance(data, str):
            data = data.encode('utf-8')  # 将字符串转换为字节
        try:
            self.serial.write(data)
        except serial.SerialException as e:
            print(f"写入数据时出错: {e}")
            self.open()


class ScatterPlot(pg.ScatterPlotItem):
    sig_ptData = pyqtSignal(dict, dict)

    def __init__(self, *args, **kargs):
        super().__init__(*args, **kargs)

    def mouseClickEvent(self, ev):
        if ev.button() == QtCore.Qt.MouseButton.LeftButton:
            pts = self.pointsAt(ev.pos())
            if len(pts) > 0:
                # print(pts[0])
                dic2 = {}
                if len(pts) > 1:
                    dic2 = pts[1].data()
                self.sig_ptData.emit(pts[0].data(), dic2)
                self.ptsClicked = pts
                ev.accept()
                self.sigClicked.emit(self, self.ptsClicked, ev)
            else:
                print("no spots")
                ev.ignore()
        else:
            ev.ignore()


class LineSegmentROI(pg.LineSegmentROI):
    sig_lineData = pyqtSignal(dict)
    color = (0, 0, 255, 155)
    hovercolor = (255, 0, 0, 100)
    pen = pg.mkPen(color=color, width=7)
    hoverPen = pg.mkPen(color=hovercolor, width=7)
    handlePen = pg.mkPen(color=color, width=7)
    handleHoverPen = pg.mkPen(color=hovercolor, width=7)
    sig_removeSelf = pyqtSignal(object)

    def __init__(self,
                 data='',
                 pen=pen,
                 hoverPen=hoverPen,
                 handleHoverPen=handleHoverPen,
                 handlePen=handlePen,
                 movable=True,
                 rotatable=True,
                 resizable=True,
                 removable=False,
                 *args,
                 **kargs):

        super().__init__(movable=movable, pen=pen, hoverPen=hoverPen,
                         handleHoverPen=handleHoverPen, handlePen=handlePen, *args, **kargs)
        self.data = data

    def mouseClickEvent(self, ev):
        if ev.button() == QtCore.Qt.MouseButton.LeftButton:
            # print(self.data)
            self.sig_lineData.emit(self.data)
            # self.sig_removeSelf.emit(self)

    # def mouseDragEvent(self, ev):
    #     pass
    def movePoint(self, handle, pos, modifiers=None, finish=True, coords='parent'):
        print("movePoint")
        pass


class PaintRoute(pg.PlotWidget):  # 运动轨迹绘制
    sig_lineData = pyqtSignal(dict)

    def __init__(self, title="AGV 运动轨迹", background='w', parent=None):
        super().__init__(parent, title=title, background=background)
        self.lines = []  # 存储所有线段的列表
        # 设置图表
        self.setXRange(-10, 10)
        self.setYRange(-10, 10)
        self.showGrid(x=True, y=True, alpha=0.5)

        self.dotdata = []
        color = (0, 0, 255, 100)
        self.scatter = ScatterPlot(
            size=10,
            brush=pg.mkBrush(color),
            pen=pg.mkPen(color),
            # pxMode=False,
            hoverable=True,
            hoverPen=pg.mkPen('b'),
            hoverBrush=pg.mkBrush('b'),
            hoverSize=10)
        self.addItem(self.scatter)

        # self.scene().sigMouseClicked.connect(self.on_mouse_click)
        # AGV车身位置
        color = (0, 255, 0, 100)
        self.agvModel = pg.ScatterPlotItem(size=20, symbol='o', pen=pg.mkPen(
            color=color), brush=pg.mkBrush(color=color))
        self.addItem(self.agvModel)
        x = AgvParameters.agvState['agvPosition']['x']
        y = AgvParameters.agvState['agvPosition']['y']
        self.agvPos = [{'pos': (x, y)}]
        self.agvModel.setData(self.agvPos)
        # AGV运动轨迹
        color = (100, 100, 100, 100)
        self.agvTrajectory = pg.ScatterPlotItem(
            size=10, symbol='o', pen=pg.mkPen(color=color), brush=pg.mkBrush(color=color))
        self.addItem(self.agvTrajectory)
        self.agvTrajectoryDot = []

    def on_mouse_click(self, event):
        # 这里我们只是打印消息，而不是删除线条
        # 实际上，你需要实现一些逻辑来确定点击是否在线条上
        print(f"Mouse clicked at position: {event.pos()}")

    def refresh_dot_agvTrajectoryDot(self):
        self.agvTrajectory.setData(self.agvTrajectoryDot)

    def refresh_dot_order(self):
        self.scatter.setData(self.dotdata)

    def show_lineData(self, data):
        self.sig_lineData.emit(data)

    def remove_line(self, line):
        self.removeItem(line)

    def remove_last_line(self):
        if self.lines:
            last_line = self.lines.pop()  # 从列表中移除最后一个线段
            self.removeItem(last_line)  # 从图表中移除线段


class widget_standly(QWidget, Ui_widget_standly.Ui_Form):  # 配置窗口

    def __init__(self):
        super().__init__()
        self.setupUi(self)
# endregion


class ROSQtNode(QMainWindow, Ui_widget_main.Ui_MainWindow):  # 主窗口
    # region
    sig_print = pyqtSignal(str, str, bool)
    app = Flask(__name__)
    # endregion

    def __init__(self):
        # region # 初始化
        # super().__init__()
        super(ROSQtNode, self).__init__()
        self.en_simulate = False
        # 订阅话题 '/chatter'，消息类型为 String，回调函数为 callback
        rospy.Subscriber('/chatter', String, self.callback)
        rospy.Subscriber("/my_imu", Float32, self.callback_imu)
        self.setupUi(self)
        self.setFocusPolicy(Qt.FocusPolicy.StrongFocus)  # 设置获取焦点的方法
        # endregion
        # region # 网页端控制
        # self.WebControl = WebControl()
        # self.WebControl.start()

        # thread = threading.Thread(target=self.run_flask, daemon=True)
        # thread.start()
        # signal_emitter.flaskWeb_signal.connect(self.on_flask_signal)
        # endregion
        # region # 运行时间测试
        # self.thread_testTime = thread_testTime()
        # self.thread_testTime.start()
        # self.thread_testTime = QTimer()
        # self.thread_testTime.timeout.connect(self.fuc_testTime)
        # self.thread_testTime.start(1000)
        # print(globals())
        # endregion
        # region # 初始化参数
        self.headerId_factsheet = 0
        self.headerId_state = 0
        self.headerId_order = 0
        self.orderId = 0
        self.orderState = 'waiting'
        self.start_time = 0
        self.end_time = 0
        # endregion
        # region # 加载配置文件
        # print("当前工作目录:", os.getcwd())
        # if os.name == 'nt':  # Windows系统
        #     os.chdir(r'\Code\mqtt_client')
        #     fileName = './config/AgvState.json'
        # elif os.name == 'posix' and virtual_machine==False:  # Linux系统
        #     print("Linux系统")
        #     fileName = '/home/sunnybaer/theyn/mqtt_client/config/AgvState.json'
        # with open(fileName, 'r') as f:
        #     data = json.load(f)
        global AgvState
        AgvState = AgvParameters.agvState
        self.manufacturer = AgvState['manufacturer']
        self.serialNumber = AgvState['serialNumber']
        # endregion
        # region # 子界面
        self.widget_standly = widget_standly()
        self.widget_standly.show()
        # endregion
        # region # 二维码数据初始化
        if os.name == 'nt':  # Windows系统
            os.chdir(r'\Code\mqtt_client')
            fileName = './config/DmCodes.xlsx'
        elif controller == "jihecheng":
            fileName = '/home/robot/Desktop/theyn/catkin_ws/src/car_pkg/src/parameter/20250409三楼测试场地.xlsx'
        elif virtual_machine == True:  # 虚拟机
            print(os.getcwd())
            print("虚拟机,Linux系统")
            fileName = '/home/theyn/Desktop/mqtt_client/config/DmCodes.xlsx'
        elif os.name == 'posix' and virtual_machine == False:  # Linux系统
            print("Linux系统")
            fileName = '/home/baer/mqtt_client/config/DmCodes.xlsx'
        df = pd.read_excel(fileName)
        array = df.values  # 或者使用 df.to_numpy()
        columns = ['序号', 'tag值', 'X(mm)', 'Y(mm)', '误差角度(°)']
        rows_dict = [dict(zip(columns, row))
                     for row in array]  # 将每行数据转换为字典，并存入列表
        self.dmCodes = {row['tag值']: row for row in rows_dict}  # 创建以tag值为键的字典

        self.dmCodes_rectify = {}
        # endregion
        # region # 日志管理
        self.sig_print.connect(self.sigCall_print)
        self.btn_clearLog.clicked.connect(self.fuc_clearLog)
        # endregion
        # region # 通讯总线管理

        self.flag_connect_mqtt = False  # 调度系统

        self.flag_connect_handler = False  # 手柄
        self.flag_connect_camera = False  # 导航相机

        self.flag_connect_chasis = False  # 底盘
        self.flag_connect_lifter = False  # 升降机构

        self.flag_connect_IMU = False  # 传感器IMU
        self.flag_connect_motor = False  # 电机

        # self.connect_lifter()  # TCP客户端(升降机构)
        # self.connect_chasis()  # TCP客户端(底盘)
        self.hostip = extract_ip()
        print(self.hostip)
        # self.listen_PC(DeviceParameters.IP_camera, 6001, 0x5043)  # TCP服务器(上位机)
        self.listen_Camera(DeviceParameters.IP_camera, 6005, 0xffff)  # TCP服务器(相机)

        self.serial_CAN = SerialPort(
            port=DeviceParameters.serial_CAN, baudrate=115200, device_type=0x4341)
        self.serial_CAN.data_received.connect(self.receive_data_handle)
        self.serial_CAN.start()
        self.serial_handle = SerialPort(
            port=DeviceParameters.serial_handle, baudrate=115200, device_type=0x4844)
        self.serial_handle.data_received.connect(self.receive_data_handle)
        self.serial_handle.start()

        # self.serial_IMU = SerialPort(
        #     port='/dev/ttyUSB1', baudrate=921600, device_type=0x4341)
        # self.serial_IMU.data_received.connect(self.analysisIMU)
        # self.serial_IMU.start()
        # print(imu.UsePlatform())
        # imu.opt=imu.parse_opt()
        # print(imu.opt)
        # imu.tr = threading.Thread(target=imu.receive_data)
        # imu.tr.start()

        self.deviceStatusChecker = DeviceStatusChecker()  # 设备状态检查器
        # self.deviceStatusChecker.sig_reconnect.connect(self.fuc_reconnect)
        # endregion
        # region # MQTT服务器
        self.status_connect = False
        self.flag_send_state = True
        self.btn_server.clicked.connect(self.connect_server)
        self.btn_topic_subscribe.clicked.connect(self.subscribe_topic)
        self.btn_topic_unsubscribe.clicked.connect(self.unsubscribe_topic)
        self.btn_topic_publish.clicked.connect(self.publish_topic)

        self.btn_clearPublish.clicked.connect(self.clearPublish)
        self.btn_test_instantActions.clicked.connect(self.test_instantActions)
        self.btn_test_order.clicked.connect(self.test_order)

        # 带复选框的下拉框
        # ccb=QComboCheckBox()
        # ccb.add_item('选项1')
        # self.horizontalLayout_2.addWidget(ccb)

        # 自动补全到文本输入控件
        wordList = [
            "Apple", "Banana", "Cherry", "Test/Topic1", "Elderberry", "Fig", "Grape", "Honeydew", "hlly", "hlly1", "Ivy gourd", "Jackfruit", "topic",
            "Topic"
        ]
        self.completer = QCompleter(wordList)
        self.completer.setCaseSensitivity(False)
        self.input_topic_name.setCompleter(self.completer)
        # endregion
        # region # TCP服务器(手柄)
        # self.hostip = '10.0.0.217'
        # self.hostip = extract_ip()
        # print(self.hostip)
        # self.server_handler = ServerThread(self.hostip, 6002)
        # self.server_handler.start()
        # self.server_handler.server_log.connect(self.fuc_log)
        # self.server_handler.sig_data_process.connect(self.handler_process)
        self.trig_speed = 0
        self.handler_last = {
            'AL1': 0,
            'AL2': 0,
            'AL3': 0,
            'AL4': 0,
            'L1': True,  # 遥杆键
            'LM': True,
            'LU': True,
            'LD': True,
            'LL': True,
            'LR': True,
        }

        self.handler = {
            'AL1': 0,
            'AL2': 0,
            'AL3': 0,
            'AL4': 0,
            'L1': True,  # 遥杆键
            'LM': True,
            'LU': True,
            'LD': True,
            'LL': True,
            'LR': True,
        }
        # endregion
        # region # 绘图窗口(显示节点和线信息)
        self.endPoints = []
        self.paint = PaintRoute()  # 运动轨迹窗口
        self.layout_paint.addWidget(self.paint)

        self.paint.scatter.sig_ptData.connect(self.show_ptData)
        self.baseLines = []

        self.last_clickDotIndex = 0
        self.last_clickLineIndex = 0
        self.comboBox_nodeList.activated.connect(self.fuc_showNodeData)
        # self.comboBox_edgeList.currentIndexChanged.connect(self.fuc_showEdgeData)
        self.comboBox_edgeList.activated.connect(self.fuc_showEdgeData)
        # endregion
        # region # 运动模拟
        # self.timer_move = QTimer(self)
        # self.timer_move.timeout.connect(self.fuc_motion_simulation)
        # self.timer_move.start(10)
        # threading.Thread(target=self.fuc_motion_simulation).start()
        self.agv_posX = AgvState['agvPosition']['x']
        self.agv_posY = AgvState['agvPosition']['y']
        self.agv_posX_last = AgvState['agvPosition']['x']
        self.agv_posY_last = AgvState['agvPosition']['y']
        self.agv_theta = AgvState['agvPosition']['theta']
        self.agv_angle = self.agv_theta * (180 / math.pi)
        self.lineEdit_agv_posX.setText(str(self.agv_posX))
        self.lineEdit_agv_posY.setText(str(self.agv_posY))
        self.lineEdit_agv_angle.setText(str(self.agv_angle))
        self.linearSpeed = 0
        self.angularSpeed = 0
        self.movePeriod = 10
        self.multiLinearSpeed = 1  # 线速度倍率(提高模拟效率)
        self.multiangularSpeed = 1  # 角速度倍率(提高模拟效率)
        self.creepSpeed = 0.03  # 爬行速度
        self.decDistance = 0  # 提前减速距离
        self.travelDistance_C = 0  # 当前行驶距离
        self.travelDistance_T = 0  # 目标行驶距离
        self.rotateTheta_C = 0  # 当前旋转角度
        self.rotateTheta_T = 0  # 目标旋转角度
        self.node_num_C = 0  # 当前节点编号
        self.node_num_T = 0  # 所有节点个数
        self.edge_num_C = 0  # 当前节点编号
        self.edge_num_T = 0  # 所有节点个数
        self.done_edge = False  # 是否完成当前线段
        self.mode_motion = 'waiting'  # 等待waiting, 直线运动move, 旋转rotate
        self.allow_deviation = 0.07 * self.multiLinearSpeed  # 距离误差
        self.btn_forward.clicked.connect(self.fuc_forward)
        self.btn_backward.clicked.connect(self.fuc_backward)
        self.btn_turnLeft.clicked.connect(self.fuc_turnLeft)
        self.btn_turnRight.clicked.connect(self.fuc_turnRight)
        self.btn_stop.clicked.connect(self.fuc_stop)

        self.btn_clear_Trajectory.clicked.connect(self.clear_Trajectory)
        self.checkBox_en_simulate.toggled.connect(self.fuc_en_simulate)
        # endregion
        # region # 电机驱动
        # self.MotorDriveType = "CAN"
        self.MotorDriveType = "UsbToCAN"
        if self.MotorDriveType == "CAN":
            self.MotorDrive = MotorDrive()
            self.MotorDrive.start()
            self.MotorDrive.signal_send_canData.connect(self.fuc_send_canData)
        elif self.MotorDriveType == "UsbToCAN":
            pass
        # endregion
        # region # 里程计&速度曲线控制&纠偏
        # self.Odometer = thread_testTime()
        self.Odometer = Odometer()
        self.Odometer.MotorDriveType = self.MotorDriveType
        self.Odometer.start()
        self.Odometer.sig_updateAgvPos.connect(self.updateAgvPos)
        self.Odometer.sig_updateAgvSpeed.connect(self.updateAgvSpeed)
        self.Odometer.sig_updateQrcode.connect(self.updateQrcode)
        self.Odometer.sig_updateMotionDone.connect(self.updateMotionDone)
        self.Odometer.sig_updateStandly.connect(self.updatetandly)
        self.widget_standly.pushButton_parameter_query.clicked.connect(
            self.fuc_queryStandly)
        self.widget_standly.pushButton_parameter_set.clicked.connect(
            self.fuc_setStandly)
        self.widget_standly.pushButton_parameter_save.clicked.connect(
            self.fuc_saveStandly)
        self.widget_standly.pushButton_parameter_load.clicked.connect(
            self.fuc_loadStandly)
        self.widget_standly.checkBox_en_standly.toggled.connect(
            self.fuc_enStandly)
        fileName = parent_dir_path + "/appDoc/standly/default.txt"
        self.fuc_initStandly(fileName)
        self.fuc_setStandly()
        self.widget_standly.pushButton_QRcode_rectify.clicked.connect(
            self.QRcode_rectify)
        # endregion
        # region # 按键监听线程
        self.keyIsPressed = False
        self.AppIsFocus = False
        # listener = keyboard.Listener(on_press=self.on_press, on_release=self.on_release)
        # listener.start()
        # endregion
        # region # 初始化后操作
        self.input_ip.setText(DeviceParameters.IP_mqtt)
        self.connect_server()  # 连接服务器
        # self.MotorDrive.configMotor(0x01)  # 配置电机
        pass
        # endregion
    # region # 运行时间测试

    def fuc_testTime(self):
        # print(get_time())
        # self.serial_CAN.write(b'hlly')
        if self.socketClient1.running:
            data = b"hlly1"
            # data=bytes(data)
            self.socketClient1.send_message(data)
        if self.socketClient2.running:
            data = b"hlly2"
            # data=bytes(data)
            self.socketClient2.send_message(data)
    # endregion
    # region # flask网页端控制

    def on_flask_signal(self, message):
        if message == 'stop':
            self.fuc_stop()
        elif message == 'forward':
            self.fuc_forward()
        elif message == 'back':
            self.fuc_backward()
        elif message == 'turnLeft':
            self.fuc_turnLeft()
        elif message == 'turnRight':
            self.fuc_turnRight()

    def run_flask(self):
        # Flask 应用默认运行在 127.0.0.1:5000
        self.app.run(host='0.0.0.0', port=5003, debug=False, threaded=True)

    @app.route('/')
    def index():
        return render_template('index.html')

    @app.route('/stop', methods=['POST'])
    def stop_task():
        signal_emitter.flaskWeb_signal.emit('stop')
        return 'stop'

    @app.route('/forward', methods=['POST'])
    def forward_task():
        signal_emitter.flaskWeb_signal.emit('forward')
        return 'forward'

    @app.route('/back', methods=['POST'])
    def back_task():
        signal_emitter.flaskWeb_signal.emit('back')
        return 'back'

    @app.route('/turnLeft', methods=['POST'])
    def turnLeft_task():
        signal_emitter.flaskWeb_signal.emit('turnLeft')
        return 'turnLeft'

    @app.route('/turnRight', methods=['POST'])
    def turnRight_task():
        signal_emitter.flaskWeb_signal.emit('turnRight')
        return 'turnRight'

    @app.route('/setspeed', methods=['POST'])
    def setspeed_task():
        # signal_emitter.flaskWeb_signal.emit('turnRight')
        data = request.json
        linearSpeed = data.get('linearSpeed', 'Not provided')
        linearSpeed = float(linearSpeed.rstrip('%')) / 100
        print(linearSpeed)
        return 'turnRight'

    # endregion
    # region # 日志管理

    def fuc_log(self, text="", type="", save=False):  # 日志显示区打印
        self.sig_print.emit(text, type, save)

    def sigCall_print(self, text="", classify="", save=False):
        # 定义ANSI转义序列
        red = '\x1b[31m'
        yellow = '\x1b[33m'
        blue = '\x1b[34m'
        reset = '\x1b[0m'  # 重置颜色
        # log = get_time() + "  " + text
        # log=f"{red}{text}{reset}"
        log = f'<span style="color: blue;">{get_time()}</span> {text}'
        self.textBrowser.append(log)
        if save:
            self.fuc_savelog(get_time() + '  ' + text, classify)

    def fuc_clearLog(self, text=""):  # 日志显示区清空
        self.textBrowser.clear()

    def fuc_savelog(self, text, classify):  # 保存日志
        if True:
            dir_name = f'log/{classify}'
            if not os.path.isdir(dir_name):
                os.makedirs(dir_name)
            data_today = datetime.now().strftime('20%y-%m-%d')
            filepath = "{0}/{1}.txt".format(dir_name, data_today)
            with open(filepath, "a+", encoding='utf-8') as f:
                f.write("{}\n".format(text))  # 自带文件关闭功能，不需要再写f.close()

    def fuc_updateState(self, text=""):  # 状态显示区更新
        self.label_state.setText(text)

    # endregion
    # region # 服务器控制（作为服务器）

    def connect_server(self):  # 启动服务器
        if self.status_connect == False:

            # self.serverIp = "10.0.0.217"
            # self.serverIp = "192.168.16.84"
            self.serverIp = self.input_ip.text()
            self.serverPort = int(self.input_port.text())
            self.serverID = self.input_clientID.text()
            self.mqtt_thread = MqttThread(
                self.serverIp, self.serverPort, self.serverID)
            # self.mqtt_thread.received_data.connect(self.show_received_data)
            self.mqtt_thread.server_log.connect(self.fuc_log)
            self.mqtt_thread.sig_connect_feedback.connect(
                self.connect_server_feedback)
            self.mqtt_thread.sig_data_analysis.connect(self.analysis_data)
            try:
                self.mqtt_thread.start()
                self.btn_server.setText("断开服务器")
                self.status_connect = True
            except Exception as e:
                self.fuc_log(f"连接服务器失败，原因：{e}")
        else:
            self.btn_server.setText("连接服务器")
            self.status_connect = False
            self.mqtt_thread.stop()
            self.timer_heartbeat.stop()
            self.com_topic_list.clear()

    def connect_server_feedback(self, status):
        if status:
            print("连接服务器成功")
            # 开启定时心跳
            self.timer_heartbeat = QTimer(self)
            self.timer_heartbeat.timeout.connect(self.send_heartbeat)
            self.timer_heartbeat.start(100)

            # 订阅默认话题
            self.subscribe(f'Test/Topic1')
            self.subscribe(
                f'robot/v2/{self.manufacturer}/{self.serialNumber}/instantActions')
            self.subscribe(
                f'robot/v2/{self.manufacturer}/{self.serialNumber}/order')
            self.subscribe(
                f'robot/v2/{self.manufacturer}/{self.serialNumber}/factsheet')
            # self.subscribe(f'robot/v2/{self.manufacturer}/{self.serialNumber}/connection')
            # self.subscribe(f'robot/v2/{self.manufacturer}/{self.serialNumber}/state')
            topic = f'robot/v2/{self.manufacturer}/{self.serialNumber}/connect'
            content = AgvParameters.connection
            content['headerId'] = 1
            content['timestamp'] = get_timestamp()
            content = json.dumps(content)
            self.publish(topic, content)
        else:
            self.btn_server.setText("连接服务器")
            self.status_connect = False
            self.mqtt_thread.stop()
            print("连接服务器失败")
            time.sleep(3)
            if self.checkBox_mode_manual.isChecked():
                return
            # self.connect_server()  #重新连接

    def send_heartbeat(self):
        if self.flag_send_state:
            topic = f'robot/v2/{self.manufacturer}/{self.serialNumber}/state'
            self.headerId_state = update_headerId(self.headerId_state)
            AgvState['headerId'] = self.headerId_state
            AgvState['timestamp'] = get_timestamp()
            content = json.dumps(AgvState)
            self.publish(topic, content)
        self.flag_send_state = True

    def subscribe(self, topic):
        if self.status_connect:
            if topic not in [self.com_topic_list.itemText(i) for i in range(self.com_topic_list.count())]:
                self.com_topic_list.addItem(topic)
                self.mqtt_thread.subscribe(topic)
            else:
                self.fuc_log(f"已经订阅话题'{topic}' 。")
        else:
            self.fuc_log("订阅默认话题失败")

    def subscribe_topic(self):
        if self.status_connect:
            topic = self.input_topic_name.text()

            if topic not in [self.com_topic_list.itemText(i) for i in range(self.com_topic_list.count())]:
                self.com_topic_list.addItem(topic)
                self.mqtt_thread.subscribe(topic)
            else:
                self.fuc_log(f"已经订阅话题'{topic}' 。")
        else:
            self.fuc_log("请先连接服务器")

    def unsubscribe_topic(self):
        if self.status_connect:
            topic = self.com_topic_list.currentText()
            self.mqtt_thread.unsubscribe(topic)
            self.com_topic_list.removeItem(self.com_topic_list.findText(topic))
        else:
            self.fuc_log("请先连接服务器")

    def publish(self, topic, content):
        isShow = self.check_show_publish.isChecked()
        self.mqtt_thread.publish(topic, content, isShow)

    def publish_topic(self):
        if self.status_connect:
            topic = self.input_topic_name.text()
            data = self.text_topic_content.toPlainText()
            isShow = self.check_show_publish.isChecked()
            self.mqtt_thread.publish(topic, data, isShow)
        else:
            self.fuc_log("请先连接服务器")

    def clearPublish(self):
        self.text_topic_content.clear()

    def test_instantActions(self):
        self.input_topic_name.setText(
            f'robot/v2/{self.manufacturer}/{self.serialNumber}/instantActions')
        content = AgvParameters.testInstantActions
        self.text_topic_content.setText(json.dumps(content))
        self.publish_topic()

    def test_order(self):
        self.input_topic_name.setText(
            f'robot/v2/{self.manufacturer}/{self.serialNumber}/order')
        content = AgvParameters.testOrder
        self.text_topic_content.setText(json.dumps(content))
        self.publish_topic()

    def analysis_data(self, topic, json_str):
        if self.check_show_json.isChecked():
            try:
                # print('收到数据')
                # json_str = '{"name": "John", "age": 30, "city": "New York"}'
                data = json.loads(json_str)
                # self.fuc_log(f"成功解析 {topic} 的数据")
                # str_data=''
                # for key, value in data.items():
                #     str_data += f"{key}: {value}"+"  "
                # self.fuc_log(str_data)
                if topic == f'robot/v2/{self.manufacturer}/{self.serialNumber}/instantActions':
                    self.process_instantActions(data)
                elif topic == f'robot/v2/{self.manufacturer}/{self.serialNumber}/order':
                    self.process_order(data)
                elif topic == f'robot/v2/{self.manufacturer}/{self.serialNumber}/factsheet':
                    self.process_factsheet(data)
                else:
                    self.fuc_log("其它话题数据")
                    print(f"未处理的数据{topic} {data}")
            except json.JSONDecodeError as e:
                self.fuc_log(f"JSON解析错误: {e}")
                # print(type(json_str))
                print(json_str)

    # endregion
    # region # 调度数据处理

    def process_instantActions(self, data):
        try:
            print('hlly')
            # print(data['actions'])
            self.input_actionId.setText(data['actions'][0]['actionId'])
            self.input_actionDescription.setText(
                data['actions'][0]["actionDescription"])
            self.input_actionType.setText(data['actions'][0]["actionType"])
            self.input_actionParameters.setText(
                str(data['actions'][0]["actionParameters"]))
            if self.stackedWidget_nodeEdge.currentIndex() != 2:
                self.stackedWidget_nodeEdge.setCurrentIndex(2)
            self.reply_factsheet()
        except Exception as e:
            print(f"数据处理异常{e}")

    def process_order_debug(self, data):
        print('process_order')

    def process_factsheet(self, data):
        print('process_factsheet')
        self.Odometer.angularSpeed_T = 0
        self.Odometer.linearSpeed_T = 0

        self.orderState = 'waiting'

    def process_order(self, data):
        # region #处理新消息和新订单
        headerId_order = data['headerId']
        orderId = data['orderId']
        if self.headerId_order == headerId_order:
            print(f'收到重复订单消息{headerId_order}')
            return
        self.headerId_order = headerId_order
        if self.orderId == orderId:
            print(f'收到重复订单{self.orderId}')
            return
        self.orderId = orderId
        print(f'process_order{self.orderId}')
        AgvState['orderId'] = self.orderId
        if self.orderState == 'running':
            print('订单执行中')
            return
        self.orderState = 'running'
        # endregion
        # region #绘制地图中点和线
        self.paint.dotdata = []  # 清空点
        self.endPoints = []
        for baseLine in self.baseLines:  # 清空线
            self.paint.removeItem(baseLine)
        self.baseLines = []

        nodes = data['nodes']
        for node in nodes:
            nodeId = node['nodeId']
            nodePosition = node['nodePosition']
            newpoint = {
                'pos': (nodePosition['x'], nodePosition['y']), 'data': node}
            self.paint.dotdata.append(newpoint)
            self.endPoints.append(
                {'nodeId': nodeId, 'x': nodePosition['x'], 'y': nodePosition['y']})
        self.paint.refresh_dot_order()

        edges = data['edges']
        for index, edge in enumerate(edges):
            edgeId = edge['edgeId']
            startNodeId = edge['startNodeId']
            endNodeId = edge['endNodeId']
            for node in self.endPoints:
                if node['nodeId'] == startNodeId:
                    x1 = node['x']
                    y1 = node['y']
                    break
            for node in self.endPoints:
                if node['nodeId'] == endNodeId:
                    x2 = node['x']
                    y2 = node['y']
                    break
            baseLine = LineSegmentROI(
                positions=[[x1, y1], [x2, y2]], data=edge)
            baseLine.sig_lineData.connect(self.show_lineData)
            self.paint.addItem(baseLine)
            self.baseLines.append(baseLine)
        # endregion
        # region #更新点和线列表
        self.comboBox_nodeList.clear()
        self.nodeList = []
        for index, node in enumerate(nodes):
            self.nodeList.append(node)
            self.comboBox_nodeList.addItem(str(index))
        self.comboBox_edgeList.clear()
        self.edgeList = []
        for index, edge in enumerate(edges):
            self.edgeList.append(edge)
            self.comboBox_edgeList.addItem(str(index))

        # endregion
        # region #执行命令
        AgvState['nodeStates'] = nodes
        AgvState['edgeStates'] = edges
        threading.Thread(target=self.traverseNodeAndEdge,
                         args=(self.nodeList, self.edgeList)).start()
        # endregion

    # endregion
    # region # 回复请求

    def reply_factsheet(self):
        topic = f'robot/v2/{self.manufacturer}/{self.serialNumber}/factsheet'
        content = AgvParameters.factsheet
        self.headerId_factsheet = update_headerId(self.headerId_factsheet)
        content['headerId'] = self.headerId_factsheet
        content['timestamp'] = get_timestamp()
        content = json.dumps(content)
        self.publish(topic, content)

    # endregion
    # region # 绘图窗口(显示节点和线信息)
    def show_ptData(self, data, data1):
        # print(data)
        if self.stackedWidget_nodeEdge.currentIndex() != 0:
            self.stackedWidget_nodeEdge.setCurrentIndex(0)
        self.lineEdit_nodeId.setText(data['nodeId'])
        self.lineEdit_sequenceId.setText(str(data['sequenceId']))
        self.lineEdit_nodeDescription.setText(data['nodeDescription'])
        self.lineEdit_released.setText(str(data['released']))

        self.lineEdit_nodePosition.setText(str(data['nodePosition']))
        self.lineEdit_nodePosition_x.setText(str(data['nodePosition']['x']))
        self.lineEdit_nodePosition_y.setText(str(data['nodePosition']['y']))
        self.lineEdit_nodePosition_theta.setText(
            str(data['nodePosition']['theta']))

        self.lineEdit_actions.setText(str(data['actions']))

        if data1:
            data = data1
            self.lineEdit_nodeId_2.setText(data['nodeId'])
            self.lineEdit_sequenceId_2.setText(str(data['sequenceId']))
            self.lineEdit_nodeDescription_2.setText(data['nodeDescription'])
            self.lineEdit_released_2.setText(str(data['released']))

            self.lineEdit_nodePosition_2.setText(str(data['nodePosition']))
            self.lineEdit_nodePosition_x_2.setText(
                str(data['nodePosition']['x']))
            self.lineEdit_nodePosition_y_2.setText(
                str(data['nodePosition']['y']))
            self.lineEdit_nodePosition_theta_2.setText(
                str(data['nodePosition']['theta']))

            self.lineEdit_actions_2.setText(str(data['actions']))
        else:
            data = {
                'nodeId': '',
                'sequenceId': '',
                'nodeDescription': '',
                'released': '',
                'nodePosition': {
                    'x': '',
                    'y': '',
                    'theta': ''
                },
                'actions': []
            }
            self.lineEdit_nodeId_2.setText(data['nodeId'])
            self.lineEdit_sequenceId_2.setText(str(data['sequenceId']))
            self.lineEdit_nodeDescription_2.setText(data['nodeDescription'])
            self.lineEdit_released_2.setText(str(data['released']))

            self.lineEdit_nodePosition_2.setText('')
            self.lineEdit_nodePosition_x_2.setText(
                str(data['nodePosition']['x']))
            self.lineEdit_nodePosition_y_2.setText(
                str(data['nodePosition']['y']))
            self.lineEdit_nodePosition_theta_2.setText(
                str(data['nodePosition']['theta']))

            self.lineEdit_actions_2.setText('')

    def show_lineData(self, data):
        if self.stackedWidget_nodeEdge.currentIndex() != 1:
            self.stackedWidget_nodeEdge.setCurrentIndex(1)
        if 'edgeId' in data:
            self.lineEdit_edgeId.setText(data['edgeId'])
        if 'sequenceId' in data:
            self.lineEdit_sequenceId_3.setText(str(data['sequenceId']))
        if 'edgeDescription' in data:
            self.lineEdit_edgeDescription.setText(data['edgeDescription'])
        if 'released' in data:
            self.lineEdit_released_3.setText(str(data['released']))
        if 'startNodeId' in data:
            self.lineEdit_startNodeId.setText(data['startNodeId'])
        if 'endNodeId' in data:
            self.lineEdit_endNodeId.setText(str(data['endNodeId']))
        if 'maxSpeed' in data:
            self.lineEdit_maxSpeed.setText(str(data['maxSpeed']))
        if 'maxHeight' in data:
            self.lineEdit_maxHeight.setText(str(data['maxHeight']))
        if 'minHeight' in data:
            self.lineEdit_minHeight.setText(str(data['minHeight']))
        if 'orientation' in data:
            self.lineEdit_orientation.setText(str(data['orientation']))
        if 'orientationType' in data:
            self.lineEdit_orientationType.setText(str(data['orientationType']))
        if 'direction' in data:
            self.lineEdit_direction.setText(str(data['direction']))
        if 'rotationAllowed' in data:
            self.lineEdit_rotationAllowed.setText(str(data['rotationAllowed']))
        if 'maxRotationSpeed' in data:
            self.lineEdit_maxRotationSpeed.setText(
                str(data['maxRotationSpeed']))
        if 'trajectory' in data:
            self.lineEdit_trajectory.setText(str(data['trajectory']))
        if 'length' in data:
            self.lineEdit_length.setText(str(data['length']))
        if 'actions' in data:
            self.lineEdit_actions_3.setText(str(data['actions']))

    def fuc_showNodeData(self, index):
        self.show_ptData(self.nodeList[index], {})
        # 点击列表中的点和线，地图中的点颜色更改
        self.paint.dotdata[self.last_clickDotIndex]['brush'] = 'b'
        self.paint.dotdata[self.last_clickDotIndex]['pen'] = 'b'
        self.paint.dotdata[index]['brush'] = 'r'
        self.paint.dotdata[index]['pen'] = 'r'
        self.paint.refresh_dot_order()
        self.last_clickDotIndex = index

    def fuc_showEdgeData(self, index):
        self.show_lineData(self.edgeList[index])
        # 点击列表中的点和线，地图中的线颜色更改
        color = (0, 0, 255, 155)
        pen = pg.mkPen(color=color, width=7)
        self.baseLines[self.last_clickLineIndex].setPen(pen)
        hovercolor = (255, 0, 0, 100)
        pen = pg.mkPen(color=hovercolor, width=7)
        self.baseLines[index].setPen(pen)
        self.last_clickLineIndex = index

    # endregion
    # region # 通讯总线管理
    def fuc_reconnect(self, device):
        if device == 'chasis':
            if self.socketClient1.running:
                pass
                # self.socketClient1.stop()#超时收不到数据，自动断开
            else:
                # self.connect_chasis()
                pass
        # elif device == 'lifter':
        #     if self.socketClient2.running:
        #         pass
        #     else:
        #         self.connect_lifter()

    def connect_lifter(self):
        s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        s.settimeout(3)
        # s.settimeout(None)
        # s_ip = '10.0.0.217'
        # s_port = 5007
        s_ip = '192.168.16.11'
        s_port = 7007
        try:
            s.connect((s_ip, s_port))
            self.fuc_log(str(s.getpeername()) + "连接成功")
        except Exception as e:
            self.fuc_log(s_ip + ": " + str(s_port) +
                         " " + str(e), 'connect', True)
        self.socketClient2 = clientThread(0x4354, s)
        self.socketClient2.log.connect(self.fuc_log)
        self.socketClient2.data_received.connect(self.receive_data_handle)
        self.socketClient2.start()

    def connect_chasis(self):
        s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        s.settimeout(3)  # 超时没有收到数据，自动断开
        # s_ip = '10.0.0.217'
        # s_port = 5007
        s_ip = '192.168.16.10'
        s_port = 5007
        try:
            s.connect((s_ip, s_port))
            # s.settimeout(None)
            self.fuc_log(str(s.getpeername()) + "连接成功")
        # except socket.error as e:
        except Exception as e:
            self.fuc_log(s_ip + ": " + str(s_port) +
                         " " + str(e), 'connect', True)
        self.socketClient1 = clientThread(0x4357, s)
        self.socketClient1.log.connect(self.fuc_log)
        self.socketClient1.data_received.connect(self.receive_data_handle)
        self.socketClient1.disConnect.connect(self.servers_disConnect)
        self.socketClient1.start()

    def servers_disConnect(self, device, thread):
        self.deviceStatusChecker.status_chasis = 0
        print(device)
        print(thread)

    def listen_PC(self, ip, port, device_type):
        self.server_PC = ServerThread(ip, port, device_type)
        self.server_PC.start()
        self.server_PC.server_log.connect(self.fuc_log)
        self.server_PC.sig_data_process.connect(self.receive_data_handle)

    def listen_Camera(self, ip, port, device_type):
        self.server_camera = ServerThread(
            ip, port, device_type)
        print("camera")
        self.server_camera.start()
        self.server_camera.sig_data_process.connect(self.analysisPGV)
    # endregion
    # region # TCP服务器(手柄)

    def handler_process(self, data):
        # 帧头、帧尾、长度都对
        if ((len(data) > 8) & (len(data) == data[3]) & (data[len(data) - 1] == 0xda)):
            self.handler_apply(data[6:len(data) - 2])

    def handler_apply(self, buf):
        self.handler['AL1'] = buf[0]
        self.handler['AL2'] = buf[1]
        self.handler['AL3'] = buf[2]
        self.handler['AL4'] = buf[3]
        self.handler['LU'] = (buf[4] & 0x80)
        self.handler['LD'] = (buf[4] & 0x40)
        self.handler['LL'] = (buf[4] & 0x20)
        self.handler['LR'] = (buf[4] & 0x10)
        self.handler['LM'] = (buf[4] & 0x08)
        self.handler['L1'] = (buf[4] & 0x04)

        self.handler['RU'] = (buf[5] & 0x80)
        self.handler['RD'] = (buf[5] & 0x40)
        self.handler['RL'] = (buf[5] & 0x20)
        self.handler['RR'] = (buf[5] & 0x10)
        self.handler['RM'] = (buf[5] & 0x08)
        self.handler['R1'] = (buf[5] & 0x04)

        # 遥杆触发与回正
        if (self.handler['AL3'] > 0x7f-0x10) and (self.handler['AL3'] < 0x7f+0x10) and \
                (self.handler['AL2'] > 0x7f-0x10) and (self.handler['AL2'] < 0x7f+0x10):
            if self.trig_speed == 3:  # 遥杆在中间需要恢复0速度情况
                self.trig_speed = 2
                self.linearSpeed = 0
                self.angularSpeed = 0
        else:
            self.trig_speed = 1
            if self.handler['AL3'] > 0x7f+0x10:
                self.angularSpeed = (
                    self.handler['AL3'] - (0x7f + 0x10)) * 0.03
            elif self.handler['AL3'] < 0x7f-0x10:
                self.angularSpeed = (
                    self.handler['AL3'] - (0x7f - 0x10)) * 0.03
            if self.handler['AL2'] > 0x7f+0x10:
                self.linearSpeed = (self.handler['AL2'] - (0x7f + 0x10)) * 0.03
            elif self.handler['AL2'] < 0x7f-0x10:
                self.linearSpeed = (self.handler['AL2'] - (0x7f - 0x10)) * 0.03
        # 下发遥杆速度
        if self.trig_speed == 1 or self.trig_speed == 2:
            if self.trig_speed == 1:
                self.trig_speed = 3
            elif self.trig_speed == 2:
                self.trig_speed = 0
            self.Odometer.linearSpeed_T = self.linearSpeed
            self.Odometer.angularSpeed_T = -self.angularSpeed
            self.Odometer.flag_controlSpeed = 1

        # 按键按下
        if self.handler_last['L1'] and not self.handler['L1']:
            print("L1")
            self.Odometer.control_mode = 'mannul'
            self.label_control_mode.setText('手动')
            self.fuc_stop()
        elif self.handler_last['LU'] and not self.handler['LU']:
            self.fuc_forward()
        elif self.handler_last['LD'] and not self.handler['LD']:
            self.fuc_backward()
        elif self.handler_last['LL'] and not self.handler['LL']:
            self.fuc_turnLeft()
        elif self.handler_last['LR'] and not self.handler['LR']:
            self.fuc_turnRight()

        # 按键松开
        if (not self.handler_last['LU'] and self.handler['LU']) or \
           (not self.handler_last['LD'] and self.handler['LD']) or \
           (not self.handler_last['LL'] and self.handler['LL']) or \
           (not self.handler_last['LR'] and self.handler['LR']):
            self.fuc_stop()
        self.handler_last = self.handler.copy()

    # endregion
    # region # 数据打包发送
    def fuc_send_dataRaw(self, Type, data):
        sendData2 = bytes(data)
        if Type == 0x4341:
            self.serial_CAN.write(sendData2)

    def fuc_send_data(self, Type, cmd_id, cmd_data):
        cmd_header = [0x50, 0x43]
        cmd_length = [0x00, 0x00]
        cmd_check = [0x00]
        cmd_end = [0xda]
        cmd_length[1] = len(cmd_data) + 8
        sendData0 = cmd_header + cmd_length + cmd_id + cmd_data
        cmd_check[0] = SumCheck(sendData0, cmd_length[1] - 2)
        sendData1 = sendData0 + cmd_check + cmd_end
        sendData2 = bytes(sendData1)
        if Type == 0x4341 or Type == 0x5043:
            self.serial_CAN.write(sendData2)
    # endregion
    # region # 接收数据处理

    def receive_data_handle(self, identify, device_type, data):  # 接收数据处理
        self.receive_data_restore(identify, device_type, data)
        # print(data)
        # print(identify)
        # print(device_type)

    def receive_data_restore(self, identify, device_type, data):  # 接收数据还原（去转义）
        pattern = rb'(?<!\\);'  # 如果';'前为'\'则不进行分割
        datalist = [x for x in re.split(pattern, data) if x]  # 去掉空''
        for i in datalist:
            pattern = rb'[\\](?=;)'
            i = re.sub(pattern, b"", i)  # 如果'\'后面是';'去掉'\'
            if len(i) > 1:
                # print(len(data))
                self.receive_data_classify(identify, device_type, i)
            else:
                print(data)

    def receive_data_classify(self, identify, device_type, data):  # 接收数据分类
        tmp_buf = [0, 0]
        tmp_buf[0] = device_type >> 8 & 0x00FF
        tmp_buf[1] = device_type & 0x00FF
        # if device_type == 0x4354:
        #     self.deviceStatusChecker.time_lifter = QTime.currentTime()
        # elif device_type == 0x4357:
        #     self.deviceStatusChecker.time_chasis = QTime.currentTime()

        # if ((data[0] == 0x42) & (data[1] == 0x44)):  # BD控制板的数据
        #     self.receive_data_verify(identify, device_type, data)
        # if ((data[0] == 0x43) & (data[1] == 0x41)):  # USB转CAN
        #     self.receive_data_verify(identify, device_type, data)
        # elif ((data[0] == 0x50) & (data[1] == 0x43)):  # PC端数据
        #     self.receive_data_verify(identify, device_type, data)
        # elif ((data[0] == 0x53) & (data[1] == 0x54)):  # ST标准通用
        #     self.receive_data_verify(identify, device_type, data)
        # elif (data[0] == 0x50) or (data[0] == 0x43):  # 接收十六进制数组
        #     a = HexStrAddSpace(data.hex())
        #     self.fuc_log(str(identify) + a, 'data', True)
        if (data[0] == tmp_buf[0] and data[1] == tmp_buf[1]):
            self.receive_data_verify(identify, device_type, data)

        else:  # 接收中文或英文或其它
            try:
                a = data.decode('utf-8')
            except Exception:
                try:
                    a = data.decode('gbk')
                except:
                    a = str(data)
            self.fuc_log(str(identify) + " " + a)
        # except:
        #     self.fuc_log("接收数据分类错误")

    def receive_data_verify(self, identify, device_type, data):  # 接收数据验证
        # print(len(data))
        # if(len(data)>3):
        try:
            # 帧头、帧尾、长度都对
            if ((len(data) > 8) & (len(data) == data[3]) & (data[len(data) - 1] == 0xda)):
                # print("单帧")
                cmdId = data[4] << 8 | data[5]
                self.receive_data_apply(
                    identify, device_type, cmdId, data[6:len(data) - 2])
                # a=HexStrAddSpace(data.hex())#查看原始数据
                # self.fuc_log(str(identify)+a)
            elif ((len(data) < 300) & (len(data) > data[3])):  # 多帧数据拆分开处理
                processData = data[:data[3]]
                # self.fuc_data_verify(processData) #!!!数据合格分开后，重新验证
                # !!!数据合格分开后，重新验证，不能直接调用fuc_data_apply！！！
                self.receive_data_verify(identify, device_type, processData)
                remainData = data[data[3]:]
                # print("多帧")
                # print("总长度"+str(len(data))+'长度'+str(data[3]))
                self.receive_data_classify(identify, device_type, remainData)
                # print("剩余长度"+str(len(remainData)))
            else:
                if (data[len(data) - 1] != 0xda):
                    # self.fuc_log(str(identify)+"帧尾错误")
                    try:
                        print(data)
                        a = data.decode('utf-8')
                        print("english")
                    except Exception:
                        try:
                            # a=data.decode('gbk')
                            a = HexStrAddSpace(data.hex())
                            print("中文")
                        except:
                            a = str(data)
                    # self.fuc_log(str(identify)+HexStrAddSpace(data.hex()))
                    self.fuc_log(str(identify) + a)
                else:
                    print("数据格式错误")
                    # self.fuc_data_apply(data[4:len(data)-2])
        except:
            a = HexStrAddSpace(data.hex())
            self.fuc_log(str(identify) + a)

    def receive_data_apply(self, identify, device_type, cmdId, data):  # 接收数据应用
        if device_type == 0x4341:  # USB转CAN
            if cmdId == 0x4060:  # 速度反馈
                Linear_velocity = listToFloat(data[0:4])
                Angular_velocity = listToFloat(data[4:8])
                # self.lineEdit_linearSpeed_C.setText(str(Linear_velocity))
                # self.lineEdit_angularSpeed_C.setText(str(Angular_velocity))
                # print("线速度%f"%Linear_velocity)
                # print("角速度%f",Angular_velocity)
        elif device_type == 0x4354:
            self.fuc_log("lifter")
        elif device_type == 0x4357:
            self.fuc_log("chasis")
        elif device_type == 0x4844:  # 手柄
            if cmdId == 0x9001:
                self.handler_apply(data)
                # self.fuc_log("handle")
        elif device_type == 0x5043:  # PC端数据
            if cmdId == 0x4060:  # 速度
                Linear_velocity = listToFloat(data[0:4])
                Angular_velocity = listToFloat(data[4:8])
                self.Odometer.linearSpeed_T = Linear_velocity
                self.Odometer.angularSpeed_T = Angular_velocity
                self.Odometer.flag_controlSpeed = 1
                self.fuc_send_dataRaw(0x4341, data[:8])
        elif device_type == 0x4359:
            pass

    def analysisPGV(self, client_address, device_type, i_PGV_Data):
        # print(i_PGV_Data)
        # 初始化变量
        flag_tagChange = 0
        tmp_tagNum = 0
        tmp_posX = 0
        tmp_posY = 0
        tmp_posYaw = 0
        tmp_agv_imuAngle = 0.0

        # posX
        if i_PGV_Data[2] & 0x04:
            # posX是负数
            tmp_posX = (0xffffff - ((i_PGV_Data[5] & 0x7f) | ((i_PGV_Data[4] & 0x7f) << 7) | ((i_PGV_Data[3] & 0x7f) << 14) |
                                    ((i_PGV_Data[2] & 0x07) << 21))) * -1
        else:
            # posX是正数
            tmp_posX = (i_PGV_Data[5] & 0x7f) | ((i_PGV_Data[4] & 0x7f) << 7) | (
                (i_PGV_Data[3] & 0x7f) << 14) | ((i_PGV_Data[2] & 0x03) << 21)

        # posY
        if i_PGV_Data[6] & 0x40:
            # posY是负数
            tmp_posY = (
                (~((i_PGV_Data[7] & 0x7f) | ((i_PGV_Data[6] & 0x7f) << 7)) + 1) & 0x3fff) * -1
        else:
            # posY是正数
            tmp_posY = (i_PGV_Data[7] & 0x7f) | ((i_PGV_Data[6] & 0x7f) << 7)

        # posYAW
        if i_PGV_Data[10] & 0x40:
            # posYAW是负数
            tmp_posYaw = ((~((i_PGV_Data[11] & 0x7f) | (
                (i_PGV_Data[10] & 0x7f) << 7)) + 1) & 0x3fff) * -1
        else:
            # posYAW是正数
            tmp_posYaw = (i_PGV_Data[11] & 0x7f) | (
                (i_PGV_Data[10] & 0x7f) << 7)

        tmp_posYaw = 3600 - tmp_posYaw
        tmp_tagNum = (i_PGV_Data[17] & 0x7f) | ((i_PGV_Data[16] & 0x7f) << 7) | (
            (i_PGV_Data[15] & 0x7f) << 14) | ((i_PGV_Data[14] & 0x7f) << 21)

        # 打印调试信息
        # print(f"X位置: {tmp_posX}, Y位置: {tmp_posY}, 角度: {tmp_posYaw}, 编码: {tmp_tagNum}")
        self.lineEdit_PGV_Num.setText(str(tmp_tagNum))
        self.lineEdit_PGV_X.setText(str(tmp_posX))
        self.lineEdit_PGV_Y.setText(str(tmp_posY))
        self.lineEdit_PGV_YAW.setText(str(tmp_posYaw))
        if tmp_tagNum:
            matched_data = self.dmCodes.get(tmp_tagNum)  # 匹配tag值
            if matched_data:
                posX = (matched_data['X(mm)'] + tmp_posX / 10) / 1000
                posY = (matched_data['Y(mm)'] + tmp_posY / 10) / 1000
                posYaw = (matched_data['误差角度(°)'] + tmp_posYaw / 10)
                # print(f"X位置: {posX}, Y位置: {posY}, 角度: {tmp_posYaw}, 编码: {tmp_tagNum}")
                self.Odometer.updateOdom_PGV(posX, posY, posYaw, tmp_tagNum)
            else:
                print("匹配失败")

    def analysisIMU(self, identify, device_type, data):
        # print(identify)#/dev/ttyUSB1
        print(data)
        # check_head = data.hex()
        # # 校验帧头
        # if check_head != FRAME_HEAD:
        #     continue
    # endregion
    # region # 运动模拟

    def fuc_en_simulate(self):
        self.en_simulate = self.checkBox_en_simulate.isChecked()
        self.Odometer.flag_simulate = self.en_simulate
    # region # 按钮控制运动

    def fuc_forward(self):
        self.Odometer.linearSpeed_T = abs(
            float(self.lineEdit_linearSpeed_T.text()))
        self.Odometer.angularSpeed_T = 0
        self.Odometer.flag_controlSpeed = 1

    def fuc_backward(self):
        self.Odometer.linearSpeed_T = - \
            abs(float(self.lineEdit_linearSpeed_T.text()))
        self.Odometer.angularSpeed_T = 0
        self.Odometer.flag_controlSpeed = 1

    def fuc_turnLeft(self):
        self.Odometer.linearSpeed_T = 0
        self.Odometer.angularSpeed_T = abs(
            float(self.lineEdit_angularSpeed_T.text()))
        self.Odometer.flag_controlSpeed = 1

    def fuc_turnRight(self):
        self.Odometer.linearSpeed_T = 0
        self.Odometer.angularSpeed_T = - \
            abs(float(self.lineEdit_angularSpeed_T.text()))
        self.Odometer.flag_controlSpeed = 1

    def fuc_stop(self):
        self.Odometer.linearSpeed_T = 0
        self.Odometer.angularSpeed_T = 0
        self.Odometer.flag_controlSpeed = 1

        # endregion
    def clear_Trajectory(self):
        self.paint.agvTrajectoryDot = []
        self.paint.agvTrajectory.setData(self.paint.agvTrajectoryDot)

    def calculate_distance(self, x1, y1, x2, y2):
        distance = math.sqrt((x2 - x1)**2 + (y2 - y1)**2)
        return distance

    # endregion
    # region # 遍历节点和边
    def traverseNodeAndEdge(self, nodeList, edgeList):
        self.node_num_C = 0
        self.node_num_T = len(self.nodeList)
        self.edge_num_C = 0
        self.edge_num_T = len(self.edgeList)
        print('遍历节点和边，节点数量:%d,边数量:%d' % (self.node_num_T, self.edge_num_T))
        self.execute_node(0)

    def execute_node(self, index):
        self.node_num_C = index
        if self.nodeList[index]['released'] == False:
            print('节点%d未释放' % index)
            return
        # else:
        #     print('节点%d已释放' % index)
        x1 = self.nodeList[index]['nodePosition']['x']
        x2 = self.agv_posX
        y1 = self.nodeList[index]['nodePosition']['y']
        y2 = self.agv_posY
        distance = self.calculate_distance(x1, y1, x2, y2)
        if distance < self.allow_deviation:
            print('AGV遍历节点%d完成' % index)
        else:
            print('AGV遍历节点%d失败,偏差距离为%.3f' % (index, distance))
            self.Odometer.angularSpeed_T = 0
            self.Odometer.linearSpeed_T = 0
            self.Odometer.flag_controlSpeed = 1
            self.orderState = 'error'
            return
        # 节点执行完毕，更新节点状态
        AgvState['nodeStates'].pop(0)
        AgvState['lastNodeId'] = self.nodeList[index]['nodeId']
        AgvState['lastNodeSequenceId'] = self.nodeList[index]['sequenceId']
        self.send_heartbeat()
        self.flag_send_state = False

        # 执行下条边
        if index < self.edge_num_T:
            self.edge_num_C = index
            self.execute_edge(index)
        else:
            self.orderState = 'finish'
            print('遍历节点和边完成')

    def execute_edge(self, index):
        if self.edgeList[index]['released'] == False:
            print('边%d未释放' % index)
            return
        self.Odometer.control_mode = 'auto'
        self.label_control_mode.setText('自动')
        # region #边上旋转运动
        if (self.edgeList[index]['startNodeId'] == self.edgeList[index]['endNodeId']
                or (self.nodeList[index]['nodePosition']['x'] == self.nodeList[index + 1]['nodePosition']['x']
                    and self.nodeList[index]['nodePosition']['y'] == self.nodeList[index + 1]['nodePosition']['y'])):
            self.Odometer.linearSpeed_T = 0
            self.travelDistance_C = 0
            self.travelDistance_T = 0
            self.rotateTheta_C = 0
            agv_deviationAngle = self.nodeList[index +
                                               1]['nodePosition']['theta'] - self.agv_theta
            agv_deviationAngle = (agv_deviationAngle +
                                  math.pi) % (2 * math.pi) - math.pi
            # 目标角速度
            if agv_deviationAngle > 0:
                self.Odometer.angularSpeed_T = 0.3
            else:
                self.Odometer.angularSpeed_T = -0.3
            # 目标角度
            self.Odometer.rotateTheta_T = self.nodeList[index +
                                                        1]['nodePosition']['theta']
            self.Odometer.planTheta_last = abs(agv_deviationAngle)
            # 减速距离
            speed = 0.3
            self.Odometer.decTheta = 0.7 * speed * speed / \
                self.Odometer.agv_RateDeceleration + self.Odometer.creepTheta  # 0.7为经验值,理论上为0.5
            self.Odometer.mode_motion = 'rotateToNode'
            print('旋转，当前角度:%.3f,目标角度:%.3f,偏差角度:%.3f' %
                  (AgvState['agvPosition']['theta'], self.nodeList[index + 1]['nodePosition']['theta'], agv_deviationAngle))
        # endregion
        # region #边上直线运动
        else:
            self.Odometer.angularSpeed_T = 0
            self.rotateTheta_C = 0
            self.rotateTheta_T = 0
            self.travelDistance_C = 0

            # 路径方向
            x1 = self.agv_posX
            y1 = self.agv_posY
            x2 = self.nodeList[index + 1]['nodePosition']['x']
            y2 = self.nodeList[index + 1]['nodePosition']['y']
            x3 = self.nodeList[index]['nodePosition']['x']
            y3 = self.nodeList[index]['nodePosition']['y']
            self.Odometer.startNode_x = x3
            self.Odometer.startNode_y = y3
            self.Odometer.targetNode_x = x2
            self.Odometer.targetNode_y = y2
            print(f'起点坐标({x3}, {y3}),终点坐标({x2}, {y2}')
            path_direction = math.atan2(y2 - y3, x2 - x3) * 180 / math.pi
            path_direction = angle_rectify(path_direction)
            self.Odometer.path_direction = path_direction
            # 倒车判断
            angle_error = path_direction - self.agv_angle
            angle_error = angle_rectify(angle_error)
            if abs(angle_error) > 90:
                self.Odometer.flag_car_backward = True
            else:
                self.Odometer.flag_car_backward = False
            # 目标速度
            speed = self.edgeList[index]['maxSpeed']
            if self.Odometer.flag_car_backward:  # 倒车
                self.Odometer.linearSpeed_T = -speed
            else:
                self.Odometer.linearSpeed_T = speed
            # 减速距离
            self.Odometer.decDistance = 0.7 * speed * speed / \
                self.Odometer.agv_deceleration + self.Odometer.creepDistance  # 0.7为经验值,理论上为0.5
            self.Odometer.mode_motion = 'moveToNode'
            self.Odometer.planDistance = self.Odometer.calculate_distance(
                x1, y1, x2, y2)
            print(
                f'前进,距离目标点{self.Odometer.planDistance:.3f}米,减速距离{self.Odometer.decDistance:.3f}米')

            # 判断是否连续运动
            self.Odometer.flag_enSlowDown = True
            if (index + 1) < self.edge_num_T:
                if self.edgeList[index + 1]['startNodeId'] != self.edgeList[index + 1]['endNodeId']:
                    self.Odometer.flag_enSlowDown = False
        # endregion
        self.Odometer.flag_controlSpeed = 1
        self.done_edge = False

    # endregion
    # region # 里程计&速度曲线控制&纠偏
    def updateAgvPos(self, agv_posX, agv_posY, agv_theta):
        # region # 更新AGV位置信息
        # 用于判断距节点距离
        self.agv_posX = agv_posX
        self.agv_posY = agv_posY
        self.agv_angle = agv_theta * (180 / math.pi)
        # 可视化显示
        self.lineEdit_agv_posX.setText(str(round(agv_posX, 3)))
        self.lineEdit_agv_posY.setText(str(round(agv_posY, 3)))
        degrees = self.agv_angle
        self.lineEdit_agv_angle.setText(str(round(degrees, 1)))
        # 上报调度信息
        AgvState['agvPosition']['x'] = agv_posX
        AgvState['agvPosition']['y'] = agv_posY
        AgvState['agvPosition']['theta'] = agv_theta
        # endregion
        # region # 更新AGV地图位置
        self.agvPos = [{'pos': (agv_posX, agv_posY)}]
        self.paint.agvModel.setData(self.agvPos)
        # endregion
        # region # 更新AGV运动轨迹
        if self.check_show_Trajectory.isChecked():
            # print(len(self.paint.agvTrajectoryDot))
            if agv_posX != self.agv_posX_last or agv_posY != self.agv_posY_last:
                self.agv_posX_last = agv_posX
                self.agv_posY_last = agv_posY
                self.paint.agvTrajectoryDot.append(
                    {'pos': (agv_posX, agv_posY)})
                self.paint.refresh_dot_agvTrajectoryDot()
        # endregion

    def updateAgvSpeed(self, linearSpeed, angularSpeed):
        if self.MotorDriveType == "CAN":
            self.MotorDrive.control_motor(linearSpeed, angularSpeed)
        elif self.MotorDriveType == "UsbToCAN":
            cmd_id = [0x40, 0x60]
            cmd_data = floatToList(linearSpeed*DeviceParameters.factor_linearSpeed) + \
                floatToList(-angularSpeed*DeviceParameters.factor_angularSpeed)
            if self.en_simulate == False:
                self.fuc_send_data(0x5043, cmd_id, cmd_data)
        self.lineEdit_linearSpeed_C.setText(str(round(linearSpeed, 3)))
        self.lineEdit_angularSpeed_C.setText(str(round(angularSpeed, 3)))
        # AgvState['velocity']['x'] = self.linearSpeed
        # AgvState['velocity']['y'] = self.linearSpeed

    def update_or_add_dm_code(self, tagnum, angle):
        if tagnum in self.dmCodes_rectify:
            # 如果存在，更新误差角度
            self.dmCodes_rectify[tagnum]['误差角度(°)'] = angle
        else:
            # 如果不存在，添加新记录
            self.dmCodes_rectify[tagnum] = {
                '误差角度(°)': angle
            }

    def updateQrcode(self, tagnum, deviation_x):
        # print(f'二维码编号:{tagnum},角度修正:{deviation_x}')
        angle = self.dmCodes[tagnum]['误差角度(°)']
        self.dmCodes[tagnum]['误差角度(°)'] = angle + 10 * deviation_x
        self.update_or_add_dm_code(tagnum, angle)
        print(f'二维码编号:{tagnum},{angle}')

    def QRcode_rectify(self):
        print(self.dmCodes_rectify)
        import csv
        # 定义 CSV 文件的文件名
        csv_filename = '二维码误差角度.csv'
        print("当前工作目录:", os.getcwd())

        # 打开文件并写入数据
        with open(csv_filename, mode='w', newline='', encoding='utf-8') as csvfile:
            # 创建 CSV 写入器
            writer = csv.writer(csvfile)
            
            # 写入表头
            writer.writerow(['二维码编号', '误差角度(°)'])
            
            # 遍历字典并写入每一行数据
            for tagnum, record in self.dmCodes_rectify.items():
                writer.writerow([tagnum, record['误差角度(°)']])

        print(f"数据已成功保存到 {csv_filename}")

    def updatetandly(self, deviation_cross, deviation_angle, angularSpeed):
        self.widget_standly.lineEdit_deviation_angle.setText(
            str(deviation_angle))
        self.widget_standly.lineEdit_deviation_cross.setText(
            str(deviation_cross))
        self.widget_standly.lineEdit_angularSpeed.setText(str(angularSpeed))

    def fuc_queryStandly(self):
        K_cross = self.Odometer.K_Cross
        K_LowVelo = self.Odometer.K_LowVelo
        K_trace_angle = self.Odometer.K_trace_angle
        K_cross_angle = self.Odometer.K_cross_angle
        self.widget_standly.lineEdit_K_Cross.setText(str(K_cross))
        self.widget_standly.lineEdit_K_LowVelo.setText(str(K_LowVelo))
        self.widget_standly.lineEdit_K_trace_angle.setText(str(K_trace_angle))
        self.widget_standly.lineEdit_K_cross_angle.setText(str(K_cross_angle))
        self.fuc_log("查询成功")

    def fuc_setStandly(self):
        K_cross = float(self.widget_standly.lineEdit_K_Cross.text())
        K_LowVelo = float(self.widget_standly.lineEdit_K_LowVelo.text())
        K_trace_angle = float(
            self.widget_standly.lineEdit_K_trace_angle.text())
        K_cross_angle = float(
            self.widget_standly.lineEdit_K_cross_angle.text())
        self.Odometer.K_Cross = K_cross
        self.Odometer.K_LowVelo = K_LowVelo
        self.Odometer.K_trace_angle = K_trace_angle
        self.Odometer.K_cross_angle = K_cross_angle
        self.fuc_log("设置成功")

    def fuc_saveStandly(self):
        dir_path = parent_dir_path + "/appDoc/standly"
        if not os.path.exists(dir_path):  # 创建目录
            os.makedirs(dir_path)
        fileName, _ = QFileDialog.getOpenFileName(None, 'Open File', dir_path)
        if fileName:
            with open(fileName, "w", encoding='utf-8') as f:  # "w"覆盖写入 "a+"追加写入
                found_widget = self.widget_standly.findChildren(
                    eval('QLineEdit'))  # 找出所有QLineEdit对象
                for i in range(len(found_widget)):
                    f.write(found_widget[i].objectName() +
                            ":" + found_widget[i].text() + "\n")
                f.close()
            self.fuc_log("保存成功")

    def fuc_loadStandly(self):
        fileName, _ = QFileDialog.getOpenFileName(
            None, 'Open File', parent_dir_path + "/appDoc/standly")
        self.fuc_initStandly(fileName)

    def fuc_initStandly(self, fileName):
        if fileName:
            with open(fileName, 'r', encoding='utf-8') as f:
                lines = f.readlines()
                for line in lines:
                    if '#' in line:
                        line = line[:line.index('#')]  # 去除每行#后的内容
                    if line.strip():  # 去除字符串两端的空白字符（包括空格、换行符和制表符）
                        parameter = line.strip().split(":", 1)
                        search_string = parameter[0]
                        found_widget = self.widget_standly.findChild(
                            QLineEdit, search_string)  # 根据字符串查找widget中的对象
                        if found_widget:
                            found_widget.setText(parameter[1])
                        else:
                            found_widget = self.widget_standly.findChild(
                                QComboBox, search_string)  # 根据字符串查找widget中的对象
                            if found_widget:
                                found_widget.setCurrentText(
                                    parameter[1])  # 必须是下拉列表中的已有项
                            else:
                                found_widget = self.widget_standly.findChild(
                                    QCheckBox, search_string)
                                found_widget.setChecked(
                                    True if parameter[1] == 'True' else False)

            self.fuc_log('加载完成')
        else:
            self.fuc_log('取消加载')

    def fuc_enStandly(self):
        if self.widget_standly.checkBox_en_standly.isChecked():
            self.Odometer.flag_enStandly = True
        else:
            self.Odometer.flag_enStandly = False
        # print("flag_enStandly:",self.Odometer.flag_enStandly)

    def updateMotionDone(self):
        self.done_edge = True
        # 等待完全停止
        if self.Odometer.flag_enSlowDown:
            while self.Odometer.flag_controlSpeed == 1:
                self.Odometer.angularSpeed_C = 0
                self.Odometer.linearSpeed_C = 0
                pass
        # 边执行完毕，更新边状态
        print('AGV遍历边%d完成' % self.edge_num_C)
        AgvState['edgeStates'].pop(0)
        # 执行下个点
        self.execute_node(self.edge_num_C + 1)

    def fuc_send_canData(self, data):
        # if self.socketClient1:
        #     self.socketClient1.send_message(data)
        # print(data)
        pass

    # endregion
    # region # 定义按键监听回调函数
    def on_press(self, key):
        if self.AppIsFocus == True:
            if self.keyIsPressed == False:
                self.keyIsPressed = True
                # print("键盘按下")
                try:
                    if key.char == 'w':
                        self.fuc_forward()
                    elif key.char == 's':
                        self.fuc_backward()
                    elif key.char == 'a':
                        self.fuc_turnLeft()
                    elif key.char == 'd':
                        self.fuc_turnRight()

                except AttributeError:
                    if key == keyboard.Key.space:
                        self.fuc_stop()

    def on_release(self, key):
        if self.AppIsFocus == True:
            self.keyIsPressed = False
            # print('按键松开')

    def focusInEvent(self, event):  # 聚焦时开启键盘监听，快捷键控制AGV运动
        self.AppIsFocus = True
        # print("聚焦")

    def focusOutEvent(self, event):  # 失焦时关闭键盘监听,防止快捷键误触发AGV运动
        self.AppIsFocus = False
        # print("失焦")

    # endregion
    def callback_imu(self, msg):
        # rospy.loginfo("Received float value: %f", msg.data)
        # self.Odometer.imu_angle= msg.data
        if self.en_simulate == False:
            try:
                self.Odometer.updateIMU(msg.data)
            except:
                pass

    def callback(self, data):
        """
        回调函数，当接收到消息时被调用
        :param data: 接收到的消息
        """
        rospy.loginfo(rospy.get_caller_id() + " I heard %s", data.data)


if __name__ == '__main__':
    parent_dir_path = get_exe_directory()
    print("parent_dir_path:", parent_dir_path)
    print("当前工作目录:", os.getcwd())
    # 先初始化ROS节点
    rospy.init_node('ros_qt_node', anonymous=False)

    # 创建Qt应用
    app = QApplication(sys.argv)
    signal_emitter = SignalEmitter()
    window = ROSQtNode()
    window.show()

    # 设置ROS回调循环与Qt事件循环协同工作
    rate = rospy.Rate(1000)  # 10Hz

    while not rospy.is_shutdown():
        # 处理Qt事件
        app.processEvents()
        # rate.sleep()
